diff options
Diffstat (limited to 'extern')
369 files changed, 34766 insertions, 2184 deletions
diff --git a/extern/bullet2/CMakeLists.txt b/extern/bullet2/CMakeLists.txt index ae7d282ca55..4bf26ab3794 100644 --- a/extern/bullet2/CMakeLists.txt +++ b/extern/bullet2/CMakeLists.txt @@ -43,9 +43,9 @@ set(SRC src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp src/BulletCollision/BroadphaseCollision/btQuantizedBvh.cpp src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp - src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.cpp src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.cpp + src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp src/BulletCollision/CollisionDispatch/btBoxBoxDetector.cpp src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp src/BulletCollision/CollisionDispatch/btCollisionObject.cpp @@ -54,16 +54,21 @@ set(SRC src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp + src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.cpp src/BulletCollision/CollisionDispatch/btGhostObject.cpp + src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp + src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h src/BulletCollision/CollisionDispatch/btManifoldResult.cpp src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.cpp src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp src/BulletCollision/CollisionDispatch/btUnionFind.cpp + src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp src/BulletCollision/CollisionShapes/btBoxShape.cpp + src/BulletCollision/CollisionShapes/btBox2dShape.cpp src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.cpp src/BulletCollision/CollisionShapes/btCapsuleShape.cpp src/BulletCollision/CollisionShapes/btCollisionShape.cpp @@ -73,14 +78,16 @@ set(SRC src/BulletCollision/CollisionShapes/btConvexHullShape.cpp src/BulletCollision/CollisionShapes/btConvexInternalShape.cpp src/BulletCollision/CollisionShapes/btConvexPointCloudShape.cpp + src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp src/BulletCollision/CollisionShapes/btConvexShape.cpp + src/BulletCollision/CollisionShapes/btConvex2dShape.cpp src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.cpp src/BulletCollision/CollisionShapes/btCylinderShape.cpp src/BulletCollision/CollisionShapes/btEmptyShape.cpp src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp src/BulletCollision/CollisionShapes/btMinkowskiSumShape.cpp - src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.cpp + src/BulletCollision/CollisionShapes/btMultiSphereShape.cpp src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.cpp @@ -97,11 +104,11 @@ set(SRC src/BulletCollision/CollisionShapes/btTriangleMeshShape.cpp src/BulletCollision/CollisionShapes/btUniformScalingShape.cpp src/BulletCollision/Gimpact/btContactProcessing.cpp + src/BulletCollision/Gimpact/btGenericPoolAllocator.cpp src/BulletCollision/Gimpact/btGImpactBvh.cpp src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.cpp src/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp src/BulletCollision/Gimpact/btGImpactShape.cpp - src/BulletCollision/Gimpact/btGenericPoolAllocator.cpp src/BulletCollision/Gimpact/btTriangleShapeEx.cpp src/BulletCollision/Gimpact/gim_box_set.cpp src/BulletCollision/Gimpact/gim_contact.cpp @@ -118,25 +125,28 @@ set(SRC src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.cpp src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.cpp - src/BulletDynamics/Character/btKinematicCharacterController.cpp + src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp + src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp + src/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.cpp src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp - src/BulletDynamics/Dynamics/Bullet-C-API.cpp - src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp + src/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp src/BulletDynamics/Dynamics/btRigidBody.cpp src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.cpp + src/BulletDynamics/Dynamics/Bullet-C-API.cpp src/BulletDynamics/Vehicle/btRaycastVehicle.cpp src/BulletDynamics/Vehicle/btWheelInfo.cpp - src/BulletSoftBody/btDefaultSoftBodySolver.cpp + src/BulletDynamics/Character/btKinematicCharacterController.cpp + src/BulletSoftBody/btSoftBody.cpp src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp src/BulletSoftBody/btSoftBodyHelpers.cpp @@ -144,21 +154,16 @@ set(SRC src/BulletSoftBody/btSoftRigidCollisionAlgorithm.cpp src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp src/BulletSoftBody/btSoftSoftCollisionAlgorithm.cpp + src/BulletSoftBody/btDefaultSoftBodySolver.cpp + src/LinearMath/btAlignedAllocator.cpp src/LinearMath/btConvexHull.cpp + src/LinearMath/btConvexHullComputer.cpp src/LinearMath/btGeometryUtil.cpp src/LinearMath/btQuickprof.cpp src/LinearMath/btSerializer.cpp - # UNUSED - # src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.cpp - # src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.cpp - # src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp - # src/BulletCollision/CollisionShapes/btBox2dShape.cpp - # src/BulletCollision/CollisionShapes/btConvex2dShape.cpp - # src/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp - # src/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp - - src/Bullet-C-Api.h + + src/BulletCollision/BroadphaseCollision/btAxisSweep3.h src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h @@ -171,9 +176,9 @@ set(SRC src/BulletCollision/BroadphaseCollision/btOverlappingPairCallback.h src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h - src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h src/BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h + src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h src/BulletCollision/CollisionDispatch/btBoxBoxDetector.h src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h @@ -183,6 +188,7 @@ set(SRC src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h + src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h @@ -193,7 +199,9 @@ set(SRC src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h src/BulletCollision/CollisionDispatch/btUnionFind.h + src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h src/BulletCollision/CollisionShapes/btBoxShape.h + src/BulletCollision/CollisionShapes/btBox2dShape.h src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h src/BulletCollision/CollisionShapes/btCapsuleShape.h src/BulletCollision/CollisionShapes/btCollisionMargin.h @@ -204,15 +212,17 @@ set(SRC src/BulletCollision/CollisionShapes/btConvexHullShape.h src/BulletCollision/CollisionShapes/btConvexInternalShape.h src/BulletCollision/CollisionShapes/btConvexPointCloudShape.h + src/BulletCollision/CollisionShapes/btConvexPolyhedron.h src/BulletCollision/CollisionShapes/btConvexShape.h + src/BulletCollision/CollisionShapes/btConvex2dShape.h src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h src/BulletCollision/CollisionShapes/btCylinderShape.h src/BulletCollision/CollisionShapes/btEmptyShape.h src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h src/BulletCollision/CollisionShapes/btMaterial.h src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h - src/BulletCollision/CollisionShapes/btMultiSphereShape.h src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h + src/BulletCollision/CollisionShapes/btMultiSphereShape.h src/BulletCollision/CollisionShapes/btOptimizedBvh.h src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h @@ -233,13 +243,13 @@ set(SRC src/BulletCollision/Gimpact/btBoxCollision.h src/BulletCollision/Gimpact/btClipPolygon.h src/BulletCollision/Gimpact/btContactProcessing.h + src/BulletCollision/Gimpact/btGenericPoolAllocator.h + src/BulletCollision/Gimpact/btGeometryOperations.h src/BulletCollision/Gimpact/btGImpactBvh.h src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h src/BulletCollision/Gimpact/btGImpactMassUtil.h src/BulletCollision/Gimpact/btGImpactQuantizedBvh.h src/BulletCollision/Gimpact/btGImpactShape.h - src/BulletCollision/Gimpact/btGenericPoolAllocator.h - src/BulletCollision/Gimpact/btGeometryOperations.h src/BulletCollision/Gimpact/btQuantization.h src/BulletCollision/Gimpact/btTriangleShapeEx.h src/BulletCollision/Gimpact/gim_array.h @@ -273,14 +283,15 @@ set(SRC src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h - src/BulletDynamics/Character/btCharacterControllerInterface.h - src/BulletDynamics/Character/btKinematicCharacterController.h + src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h + src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h src/BulletDynamics/ConstraintSolver/btConstraintSolver.h src/BulletDynamics/ConstraintSolver/btContactConstraint.h src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h + src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h src/BulletDynamics/ConstraintSolver/btHingeConstraint.h src/BulletDynamics/ConstraintSolver/btJacobianEntry.h src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h @@ -290,41 +301,45 @@ set(SRC src/BulletDynamics/ConstraintSolver/btSolverBody.h src/BulletDynamics/ConstraintSolver/btSolverConstraint.h src/BulletDynamics/ConstraintSolver/btTypedConstraint.h + src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h src/BulletDynamics/Dynamics/btActionInterface.h - src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.h src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h src/BulletDynamics/Dynamics/btDynamicsWorld.h - src/BulletDynamics/Dynamics/btRigidBody.h src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h + src/BulletDynamics/Dynamics/btRigidBody.h src/BulletDynamics/Vehicle/btRaycastVehicle.h src/BulletDynamics/Vehicle/btVehicleRaycaster.h src/BulletDynamics/Vehicle/btWheelInfo.h - src/BulletSoftBody/btDefaultSoftBodySolver.h + src/BulletDynamics/Character/btCharacterControllerInterface.h + src/BulletDynamics/Character/btKinematicCharacterController.h + src/BulletSoftBody/btSoftBody.h - src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h src/BulletSoftBody/btSoftBodyData.h + src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h src/BulletSoftBody/btSoftBodyHelpers.h - src/BulletSoftBody/btSoftBodyInternals.h src/BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h - src/BulletSoftBody/btSoftBodySolverVertexBuffer.h - src/BulletSoftBody/btSoftBodySolvers.h src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h src/BulletSoftBody/btSoftRigidDynamicsWorld.h src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h src/BulletSoftBody/btSparseSDF.h + src/BulletSoftBody/btSoftBodySolvers.h + src/BulletSoftBody/btDefaultSoftBodySolver.h + src/BulletSoftBody/btSoftBodySolverVertexBuffer.h + src/LinearMath/btAabbUtil2.h src/LinearMath/btAlignedAllocator.h src/LinearMath/btAlignedObjectArray.h src/LinearMath/btConvexHull.h + src/LinearMath/btConvexHullComputer.h src/LinearMath/btDefaultMotionState.h src/LinearMath/btGeometryUtil.h + src/LinearMath/btGrahamScan2dConvexHull.h src/LinearMath/btHashMap.h src/LinearMath/btIDebugDraw.h src/LinearMath/btList.h src/LinearMath/btMatrix3x3.h src/LinearMath/btMinMax.h src/LinearMath/btMotionState.h - src/LinearMath/btPoint3.h src/LinearMath/btPoolAllocator.h src/LinearMath/btQuadWord.h src/LinearMath/btQuaternion.h @@ -332,20 +347,14 @@ set(SRC src/LinearMath/btRandom.h src/LinearMath/btScalar.h src/LinearMath/btSerializer.h - src/LinearMath/btSimdMinMax.h src/LinearMath/btStackAlloc.h src/LinearMath/btTransform.h src/LinearMath/btTransformUtil.h src/LinearMath/btVector3.h + + src/btBulletCollisionCommon.h src/btBulletDynamicsCommon.h - # src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h - # src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h - # src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.h - # src/BulletCollision/CollisionShapes/btBox2dShape.h - # src/BulletCollision/CollisionShapes/btConvex2dShape.h - # src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h - # src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h ) if(CMAKE_COMPILER_IS_GNUCXX) diff --git a/extern/bullet2/patches/ghost_softbody.patch b/extern/bullet2/patches/ghost_softbody.patch new file mode 100644 index 00000000000..b150d57040d --- /dev/null +++ b/extern/bullet2/patches/ghost_softbody.patch @@ -0,0 +1,42 @@ +Index: extern/bullet2/src/BulletSoftBody/btSoftBody.cpp +=================================================================== +--- extern/bullet2/src/BulletSoftBody/btSoftBody.cpp (Revision 43904) ++++ extern/bullet2/src/BulletSoftBody/btSoftBody.cpp (Revision 43905) +@@ -2780,21 +2780,23 @@ + { + const RContact& c = psb->m_rcontacts[i]; + const sCti& cti = c.m_cti; +- btRigidBody* tmpRigid = btRigidBody::upcast(cti.m_colObj); + +- const btVector3 va = tmpRigid ? tmpRigid->getVelocityInLocalPoint(c.m_c1)*dt : btVector3(0,0,0); +- const btVector3 vb = c.m_node->m_x-c.m_node->m_q; +- const btVector3 vr = vb-va; +- const btScalar dn = btDot(vr, cti.m_normal); +- if(dn<=SIMD_EPSILON) +- { +- const btScalar dp = btMin( (btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg ); +- const btVector3 fv = vr - (cti.m_normal * dn); +- // c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient +- const btVector3 impulse = c.m_c0 * ( (vr - (fv * c.m_c3) + (cti.m_normal * (dp * c.m_c4))) * kst ); +- c.m_node->m_x -= impulse * c.m_c2; +- if (tmpRigid) +- tmpRigid->applyImpulse(impulse,c.m_c1); ++ if (cti.m_colObj->hasContactResponse()) { ++ btRigidBody* tmpRigid = btRigidBody::upcast(cti.m_colObj); ++ const btVector3 va = tmpRigid ? tmpRigid->getVelocityInLocalPoint(c.m_c1)*dt : btVector3(0,0,0); ++ const btVector3 vb = c.m_node->m_x-c.m_node->m_q; ++ const btVector3 vr = vb-va; ++ const btScalar dn = btDot(vr, cti.m_normal); ++ if(dn<=SIMD_EPSILON) ++ { ++ const btScalar dp = btMin( (btDot(c.m_node->m_x, cti.m_normal) + cti.m_offset), mrg ); ++ const btVector3 fv = vr - (cti.m_normal * dn); ++ // c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient ++ const btVector3 impulse = c.m_c0 * ( (vr - (fv * c.m_c3) + (cti.m_normal * (dp * c.m_c4))) * kst ); ++ c.m_node->m_x -= impulse * c.m_c2; ++ if (tmpRigid) ++ tmpRigid->applyImpulse(impulse,c.m_c1); ++ } + } + } + } diff --git a/extern/bullet2/patches/pvs_warning_fixes.patch b/extern/bullet2/patches/pvs_warning_fixes.patch new file mode 100644 index 00000000000..5a3fe140454 --- /dev/null +++ b/extern/bullet2/patches/pvs_warning_fixes.patch @@ -0,0 +1,31 @@ +Index: extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h +=================================================================== +--- extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h (Revision 45907) ++++ extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h (Revision 45908) +@@ -45,7 +45,9 @@ + int getTriangleIndex() const + { + // Get only the lower bits where the triangle index is stored +- return (m_PartIdTriangleIndex&~((~0)<<(31-MAX_NUM_PARTS_IN_BITS))); ++ unsigned int x = 0; ++ unsigned int y = (~(x&0))<<(31-MAX_NUM_PARTS_IN_BITS); ++ return (m_PartIdTriangleIndex&~(y)); + } + int getPartId() const + { +Index: extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h +=================================================================== +--- extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h (Revision 45907) ++++ extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h (Revision 45908) +@@ -78,8 +78,10 @@ + int getTriangleIndex() const + { + btAssert(isLeafNode()); ++ unsigned int x=0; ++ unsigned int y = (~(x&0))<<(31-MAX_NUM_PARTS_IN_BITS); + // Get only the lower bits where the triangle index is stored +- return (m_escapeIndexOrTriangleIndex&~((~0)<<(31-MAX_NUM_PARTS_IN_BITS))); ++ return (m_escapeIndexOrTriangleIndex&~(y)); + } + int getPartId() const + { diff --git a/extern/bullet2/readme.txt b/extern/bullet2/readme.txt index e2546b049e3..e537ac26189 100644 --- a/extern/bullet2/readme.txt +++ b/extern/bullet2/readme.txt @@ -1,21 +1,15 @@ -*** -Apply bullet_compound_raycast.patch if not already applied in Bullet source -This patch is needed to return correct raycast results on compound shape. -/ben - - -*** These files in extern/bullet2 are NOT part of the Blender build yet *** - This is the new refactored version of Bullet physics library version 2.x -Soon this will replace the old Bullet version in extern/bullet. -First the integration in Blender Game Engine needs to be updated. -Once that is done all build systems can be updated to use/build extern/bullet2 files. - Questions? mail blender at erwincoumans.com, or check the bf-blender mailing list. Thanks, Erwin +Apply patches/ghost_softbody.patch to prevent softbodies being hit by ghost objects. +Originally committed in blender svn revision: 43905. + +Apply patches/pvs_warning_fixes.patch to fix warnings reported by PVS-Studio. +Originally committed in blender svn revision: 45908. + Apply patches/make_id.patch to prevent duplicated define of MAKE_ID macro in blender side and bullet side. Sergey diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h index 70cf3e4ace8..4f4d94b3cc7 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btAxisSweep3.h @@ -16,8 +16,8 @@ // // 3. This notice may not be removed or altered from any source distribution. -#ifndef AXIS_SWEEP_3_H -#define AXIS_SWEEP_3_H +#ifndef BT_AXIS_SWEEP_3_H +#define BT_AXIS_SWEEP_3_H #include "LinearMath/btVector3.h" #include "btOverlappingPairCache.h" @@ -1026,7 +1026,7 @@ void btAxisSweep3Internal<BP_FP_INT_TYPE>::sortMaxUp(int axis, BP_FP_INT_TYPE ed /// The btAxisSweep3 is an efficient implementation of the 3d axis sweep and prune broadphase. -/// It uses arrays rather than lists for storage of the 3 axis. Also it operates using 16 bit integer coordinates instead of floats. +/// It uses arrays rather then lists for storage of the 3 axis. Also it operates using 16 bit integer coordinates instead of floats. /// For large worlds and many objects, use bt32BitAxisSweep3 or btDbvtBroadphase instead. bt32BitAxisSweep3 has higher precision and allows more then 16384 objects at the cost of more memory and bit of performance. class btAxisSweep3 : public btAxisSweep3Internal<unsigned short int> { @@ -1038,7 +1038,7 @@ public: /// The bt32BitAxisSweep3 allows higher precision quantization and more objects compared to the btAxisSweep3 sweep and prune. /// This comes at the cost of more memory per handle, and a bit slower performance. -/// It uses arrays rather than lists for storage of the 3 axis. +/// It uses arrays rather then lists for storage of the 3 axis. class bt32BitAxisSweep3 : public btAxisSweep3Internal<unsigned int> { public: diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h index fe414effbfc..f1bf00594d3 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseInterface.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef BROADPHASE_INTERFACE_H -#define BROADPHASE_INTERFACE_H +#ifndef BT_BROADPHASE_INTERFACE_H +#define BT_BROADPHASE_INTERFACE_H @@ -79,4 +79,4 @@ public: }; -#endif //BROADPHASE_INTERFACE_H +#endif //BT_BROADPHASE_INTERFACE_H diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h index 62d349739c3..bb58b828936 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btBroadphaseProxy.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef BROADPHASE_PROXY_H -#define BROADPHASE_PROXY_H +#ifndef BT_BROADPHASE_PROXY_H +#define BT_BROADPHASE_PROXY_H #include "LinearMath/btScalar.h" //for SIMD_FORCE_INLINE #include "LinearMath/btVector3.h" @@ -246,7 +246,7 @@ class btBroadphasePairSortPredicate { public: - bool operator() ( const btBroadphasePair& a, const btBroadphasePair& b ) + bool operator() ( const btBroadphasePair& a, const btBroadphasePair& b ) const { const int uidA0 = a.m_pProxy0 ? a.m_pProxy0->m_uniqueId : -1; const int uidB0 = b.m_pProxy0 ? b.m_pProxy0->m_uniqueId : -1; @@ -266,5 +266,5 @@ SIMD_FORCE_INLINE bool operator==(const btBroadphasePair& a, const btBroadphaseP } -#endif //BROADPHASE_PROXY_H +#endif //BT_BROADPHASE_PROXY_H diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h index 0d8bca41c8e..36eec97174f 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef COLLISION_ALGORITHM_H -#define COLLISION_ALGORITHM_H +#ifndef BT_COLLISION_ALGORITHM_H +#define BT_COLLISION_ALGORITHM_H #include "LinearMath/btScalar.h" #include "LinearMath/btAlignedObjectArray.h" @@ -77,4 +77,4 @@ public: }; -#endif //COLLISION_ALGORITHM_H +#endif //BT_COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h index 2bb8ef5d2a7..409da80ae1b 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDbvt.h @@ -259,6 +259,7 @@ struct btDbvt btAlignedObjectArray<sStkNN> m_stkStack; + mutable btAlignedObjectArray<const btDbvtNode*> m_rayTestStack; // Methods @@ -955,7 +956,7 @@ inline void btDbvt::rayTestInternal( const btDbvtNode* root, int depth=1; int treshold=DOUBLE_STACKSIZE-2; - btAlignedObjectArray<const btDbvtNode*> stack; + btAlignedObjectArray<const btDbvtNode*>& stack = m_rayTestStack; stack.resize(DOUBLE_STACKSIZE); stack[0]=root; btVector3 bounds[2]; diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h index 8ded0006c3b..a79cf9402b1 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btDispatcher.h @@ -13,9 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef _DISPATCHER_H -#define _DISPATCHER_H - +#ifndef BT_DISPATCHER_H +#define BT_DISPATCHER_H #include "LinearMath/btScalar.h" class btCollisionAlgorithm; @@ -27,6 +26,7 @@ class btOverlappingPairCache; class btPersistentManifold; class btStackAlloc; +class btPoolAllocator; struct btDispatcherInfo { @@ -40,7 +40,7 @@ struct btDispatcherInfo m_stepCount(0), m_dispatchFunc(DISPATCH_DISCRETE), m_timeOfImpact(btScalar(1.)), - m_useContinuous(false), + m_useContinuous(true), m_debugDraw(0), m_enableSatConvex(false), m_enableSPU(true), @@ -48,7 +48,6 @@ struct btDispatcherInfo m_allowedCcdPenetration(btScalar(0.04)), m_useConvexConservativeDistanceUtil(false), m_convexConservativeDistanceThreshold(0.0f), - m_convexMaxDistanceUseCPT(false), m_stackAllocator(0) { @@ -65,7 +64,6 @@ struct btDispatcherInfo btScalar m_allowedCcdPenetration; bool m_useConvexConservativeDistanceUtil; btScalar m_convexConservativeDistanceThreshold; - bool m_convexMaxDistanceUseCPT; btStackAlloc* m_stackAllocator; }; @@ -98,6 +96,10 @@ public: virtual btPersistentManifold** getInternalManifoldPointer() = 0; + virtual btPoolAllocator* getInternalManifoldPool() = 0; + + virtual const btPoolAllocator* getInternalManifoldPool() const = 0; + virtual void* allocateCollisionAlgorithm(int size) = 0; virtual void freeCollisionAlgorithm(void* ptr) = 0; @@ -105,4 +107,4 @@ public: }; -#endif //_DISPATCHER_H +#endif //BT_DISPATCHER_H diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp index 6712f528e97..81369fe9b50 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btMultiSapBroadphase.cpp @@ -341,7 +341,7 @@ class btMultiSapBroadphasePairSortPredicate { public: - bool operator() ( const btBroadphasePair& a1, const btBroadphasePair& b1 ) + bool operator() ( const btBroadphasePair& a1, const btBroadphasePair& b1 ) const { btMultiSapBroadphase::btMultiSapProxy* aProxy0 = a1.m_pProxy0 ? (btMultiSapBroadphase::btMultiSapProxy*)a1.m_pProxy0->m_multiSapParentProxy : 0; btMultiSapBroadphase::btMultiSapProxy* aProxy1 = a1.m_pProxy1 ? (btMultiSapBroadphase::btMultiSapProxy*)a1.m_pProxy1->m_multiSapParentProxy : 0; diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h index 3945afb8d70..7a3806c1d28 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef OVERLAPPING_PAIR_CACHE_H -#define OVERLAPPING_PAIR_CACHE_H +#ifndef BT_OVERLAPPING_PAIR_CACHE_H +#define BT_OVERLAPPING_PAIR_CACHE_H #include "btBroadphaseInterface.h" @@ -464,6 +464,6 @@ public: }; -#endif //OVERLAPPING_PAIR_CACHE_H +#endif //BT_OVERLAPPING_PAIR_CACHE_H diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h index 9e247d125f5..78382da79f0 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btQuantizedBvh.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef QUANTIZED_BVH_H -#define QUANTIZED_BVH_H +#ifndef BT_QUANTIZED_BVH_H +#define BT_QUANTIZED_BVH_H class btSerializer; @@ -109,9 +109,9 @@ ATTRIBUTE_ALIGNED16 (struct) btOptimizedBvhNode //for child nodes int m_subPart; int m_triangleIndex; - int m_padding[5];//bad, due to alignment - +//pad the size to 64 bytes + char m_padding[20]; }; @@ -339,7 +339,7 @@ public: ///***************************************** expert/internal use only ************************* - void setQuantizationValues(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax,btScalar quantizationMargin=btScalar(1.5)); + void setQuantizationValues(const btVector3& bvhAabbMin,const btVector3& bvhAabbMax,btScalar quantizationMargin=btScalar(1.0)); QuantizedNodeArray& getLeafNodeArray() { return m_quantizedLeafNodes; } ///buildInternal is expert use only: assumes that setQuantizationValues and LeafNodeArray are initialized void buildInternal(); @@ -578,4 +578,4 @@ SIMD_FORCE_INLINE int btQuantizedBvh::calculateSerializeBufferSizeNew() const -#endif //QUANTIZED_BVH_H +#endif //BT_QUANTIZED_BVH_H diff --git a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h index 3e7c7ee3b62..7cb3c40a043 100644 --- a/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h +++ b/extern/bullet2/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef SIMPLE_BROADPHASE_H -#define SIMPLE_BROADPHASE_H +#ifndef BT_SIMPLE_BROADPHASE_H +#define BT_SIMPLE_BROADPHASE_H #include "btOverlappingPairCache.h" @@ -167,5 +167,5 @@ public: -#endif //SIMPLE_BROADPHASE_H +#endif //BT_SIMPLE_BROADPHASE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h index f656e5c323a..22953af43fd 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef SPHERE_TRIANGLE_DETECTOR_H -#define SPHERE_TRIANGLE_DETECTOR_H +#ifndef BT_SPHERE_TRIANGLE_DETECTOR_H +#define BT_SPHERE_TRIANGLE_DETECTOR_H #include "BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h" @@ -47,5 +47,5 @@ private: btScalar m_contactBreakingThreshold; }; -#endif //SPHERE_TRIANGLE_DETECTOR_H +#endif //BT_SPHERE_TRIANGLE_DETECTOR_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h index 21342175238..97c5be77003 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBox2dBox2dCollisionAlgorithm.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef BOX_2D_BOX_2D__COLLISION_ALGORITHM_H -#define BOX_2D_BOX_2D__COLLISION_ALGORITHM_H +#ifndef BT_BOX_2D_BOX_2D__COLLISION_ALGORITHM_H +#define BT_BOX_2D_BOX_2D__COLLISION_ALGORITHM_H #include "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" @@ -62,5 +62,5 @@ public: }; -#endif //BOX_2D_BOX_2D__COLLISION_ALGORITHM_H +#endif //BT_BOX_2D_BOX_2D__COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h index e7d2cc25c22..f0bbae61e3b 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxCollisionAlgorithm.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef BOX_BOX__COLLISION_ALGORITHM_H -#define BOX_BOX__COLLISION_ALGORITHM_H +#ifndef BT_BOX_BOX__COLLISION_ALGORITHM_H +#define BT_BOX_BOX__COLLISION_ALGORITHM_H #include "btActivatingCollisionAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" @@ -62,5 +62,5 @@ public: }; -#endif //BOX_BOX__COLLISION_ALGORITHM_H +#endif //BT_BOX_BOX__COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.h index 605294d47bd..3c941f7deb2 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btBoxBoxDetector.h @@ -16,8 +16,8 @@ subject to the following restrictions: 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ -#ifndef BOX_BOX_DETECTOR_H -#define BOX_BOX_DETECTOR_H +#ifndef BT_BOX_BOX_DETECTOR_H +#define BT_BOX_BOX_DETECTOR_H class btBoxShape; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h index 1db51a36d03..f63e0923b78 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionConfiguration.h @@ -15,6 +15,7 @@ subject to the following restrictions: #ifndef BT_COLLISION_CONFIGURATION #define BT_COLLISION_CONFIGURATION + struct btCollisionAlgorithmCreateFunc; class btStackAlloc; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h index a6da5f61a3c..1d7e74401dd 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionCreateFunc.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef COLLISION_CREATE_FUNC -#define COLLISION_CREATE_FUNC +#ifndef BT_COLLISION_CREATE_FUNC +#define BT_COLLISION_CREATE_FUNC #include "LinearMath/btAlignedObjectArray.h" class btCollisionAlgorithm; @@ -41,5 +41,5 @@ struct btCollisionAlgorithmCreateFunc return 0; } }; -#endif //COLLISION_CREATE_FUNC +#endif //BT_COLLISION_CREATE_FUNC diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp index 9fed44a19f7..29674f3be46 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.cpp @@ -92,8 +92,16 @@ btPersistentManifold* btCollisionDispatcher::getNewManifold(void* b0,void* b1) mem = m_persistentManifoldPoolAllocator->allocate(sizeof(btPersistentManifold)); } else { - mem = btAlignedAlloc(sizeof(btPersistentManifold),16); - + //we got a pool memory overflow, by default we fallback to dynamically allocate memory. If we require a contiguous contact pool then assert. + if ((m_dispatcherFlags&CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION)==0) + { + mem = btAlignedAlloc(sizeof(btPersistentManifold),16); + } else + { + btAssert(0); + //make sure to increase the m_defaultMaxPersistentManifoldPoolSize in the btDefaultCollisionConstructionInfo/btDefaultCollisionConfiguration + return 0; + } } btPersistentManifold* manifold = new(mem) btPersistentManifold (body0,body1,0,contactBreakingThreshold,contactProcessingThreshold); manifold->m_index1a = m_manifoldsPtr.size(); @@ -172,8 +180,7 @@ bool btCollisionDispatcher::needsCollision(btCollisionObject* body0,btCollisionO if (!(m_dispatcherFlags & btCollisionDispatcher::CD_STATIC_STATIC_REPORTED)) { //broadphase filtering already deals with this - if ((body0->isStaticObject() || body0->isKinematicObject()) && - (body1->isStaticObject() || body1->isKinematicObject())) + if (body0->isStaticOrKinematicObject() && body1->isStaticOrKinematicObject()) { m_dispatcherFlags |= btCollisionDispatcher::CD_STATIC_STATIC_REPORTED; printf("warning btCollisionDispatcher::needsCollision: static-static collision!\n"); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h index 3c4f039504a..5accad9a993 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionDispatcher.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef COLLISION__DISPATCHER_H -#define COLLISION__DISPATCHER_H +#ifndef BT_COLLISION__DISPATCHER_H +#define BT_COLLISION__DISPATCHER_H #include "BulletCollision/BroadphaseCollision/btDispatcher.h" #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" @@ -67,7 +67,8 @@ public: enum DispatcherFlags { CD_STATIC_STATIC_REPORTED = 1, - CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD = 2 + CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD = 2, + CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION = 4 }; int getDispatcherFlags() const @@ -90,7 +91,7 @@ public: btPersistentManifold** getInternalManifoldPointer() { - return &m_manifoldsPtr[0]; + return m_manifoldsPtr.size()? &m_manifoldsPtr[0] : 0; } btPersistentManifold* getManifoldByIndexInternal(int index) @@ -155,7 +156,17 @@ public: m_collisionConfiguration = config; } + virtual btPoolAllocator* getInternalManifoldPool() + { + return m_persistentManifoldPoolAllocator; + } + + virtual const btPoolAllocator* getInternalManifoldPool() const + { + return m_persistentManifoldPoolAllocator; + } + }; -#endif //COLLISION__DISPATCHER_H +#endif //BT_COLLISION__DISPATCHER_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h index 5de829824ff..3a11c967ac9 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionObject.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef COLLISION_OBJECT_H -#define COLLISION_OBJECT_H +#ifndef BT_COLLISION_OBJECT_H +#define BT_COLLISION_OBJECT_H #include "LinearMath/btTransform.h" @@ -521,4 +521,4 @@ SIMD_FORCE_INLINE int btCollisionObject::calculateSerializeBufferSize() const -#endif //COLLISION_OBJECT_H +#endif //BT_COLLISION_OBJECT_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp index bfe8d4f52fb..66b93b88efa 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp @@ -33,6 +33,7 @@ subject to the following restrictions: #include "LinearMath/btQuickprof.h" #include "LinearMath/btStackAlloc.h" #include "LinearMath/btSerializer.h" +#include "BulletCollision/CollisionShapes/btConvexPolyhedron.h" //#define DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION @@ -154,7 +155,7 @@ void btCollisionWorld::updateSingleAabb(btCollisionObject* colObj) minAabb -= contactThreshold; maxAabb += contactThreshold; - if(getDispatchInfo().m_convexMaxDistanceUseCPT) + if(getDispatchInfo().m_useContinuous && colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY && !colObj->isStaticOrKinematicObject()) { btVector3 minAabb2,maxAabb2; colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(),minAabb2,maxAabb2); @@ -662,68 +663,103 @@ void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const bt triangleMesh->performConvexcast(&tccb,convexFromLocal,convexToLocal,boxMinLocal, boxMaxLocal); } else { - //BT_PROFILE("convexSweepConcave"); - btConcaveShape* concaveShape = (btConcaveShape*)collisionShape; - btTransform worldTocollisionObject = colObjWorldTransform.inverse(); - btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin(); - btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin(); - // rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation - btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis()); - - //ConvexCast::CastResult - struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback + if (collisionShape->getShapeType()==STATIC_PLANE_PROXYTYPE) { - btCollisionWorld::ConvexResultCallback* m_resultCallback; - btCollisionObject* m_collisionObject; - btConcaveShape* m_triangleMesh; - - BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to, - btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& triangleToWorld): - btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()), - m_resultCallback(resultCallback), - m_collisionObject(collisionObject), - m_triangleMesh(triangleMesh) + btConvexCast::CastResult castResult; + castResult.m_allowedPenetration = allowedPenetration; + castResult.m_fraction = resultCallback.m_closestHitFraction; + btStaticPlaneShape* planeShape = (btStaticPlaneShape*) collisionShape; + btContinuousConvexCollision convexCaster1(castShape,planeShape); + btConvexCast* castPtr = &convexCaster1; + + if (castPtr->calcTimeOfImpact(convexFromTrans,convexToTrans,colObjWorldTransform,colObjWorldTransform,castResult)) { + //add hit + if (castResult.m_normal.length2() > btScalar(0.0001)) + { + if (castResult.m_fraction < resultCallback.m_closestHitFraction) + { + castResult.m_normal.normalize(); + btCollisionWorld::LocalConvexResult localConvexResult + ( + collisionObject, + 0, + castResult.m_normal, + castResult.m_hitPoint, + castResult.m_fraction + ); + + bool normalInWorldSpace = true; + resultCallback.addSingleResult(localConvexResult, normalInWorldSpace); + } + } } - - virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex ) + } else + { + //BT_PROFILE("convexSweepConcave"); + btConcaveShape* concaveShape = (btConcaveShape*)collisionShape; + btTransform worldTocollisionObject = colObjWorldTransform.inverse(); + btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin(); + btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin(); + // rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation + btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis()); + + //ConvexCast::CastResult + struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback { - btCollisionWorld::LocalShapeInfo shapeInfo; - shapeInfo.m_shapePart = partId; - shapeInfo.m_triangleIndex = triangleIndex; - if (hitFraction <= m_resultCallback->m_closestHitFraction) + btCollisionWorld::ConvexResultCallback* m_resultCallback; + btCollisionObject* m_collisionObject; + btConcaveShape* m_triangleMesh; + + BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to, + btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& triangleToWorld): + btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()), + m_resultCallback(resultCallback), + m_collisionObject(collisionObject), + m_triangleMesh(triangleMesh) { + } - btCollisionWorld::LocalConvexResult convexResult - (m_collisionObject, - &shapeInfo, - hitNormalLocal, - hitPointLocal, - hitFraction); - bool normalInWorldSpace = false; + virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex ) + { + btCollisionWorld::LocalShapeInfo shapeInfo; + shapeInfo.m_shapePart = partId; + shapeInfo.m_triangleIndex = triangleIndex; + if (hitFraction <= m_resultCallback->m_closestHitFraction) + { - return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace); - } - return hitFraction; - } + btCollisionWorld::LocalConvexResult convexResult + (m_collisionObject, + &shapeInfo, + hitNormalLocal, + hitPointLocal, + hitFraction); - }; + bool normalInWorldSpace = false; - BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,concaveShape, colObjWorldTransform); - tccb.m_hitFraction = resultCallback.m_closestHitFraction; - tccb.m_allowedPenetration = allowedPenetration; - btVector3 boxMinLocal, boxMaxLocal; - castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal); + return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace); + } + return hitFraction; + } + + }; - btVector3 rayAabbMinLocal = convexFromLocal; - rayAabbMinLocal.setMin(convexToLocal); - btVector3 rayAabbMaxLocal = convexFromLocal; - rayAabbMaxLocal.setMax(convexToLocal); - rayAabbMinLocal += boxMinLocal; - rayAabbMaxLocal += boxMaxLocal; - concaveShape->processAllTriangles(&tccb,rayAabbMinLocal,rayAabbMaxLocal); + BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,concaveShape, colObjWorldTransform); + tccb.m_hitFraction = resultCallback.m_closestHitFraction; + tccb.m_allowedPenetration = allowedPenetration; + btVector3 boxMinLocal, boxMaxLocal; + castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal); + + btVector3 rayAabbMinLocal = convexFromLocal; + rayAabbMinLocal.setMin(convexToLocal); + btVector3 rayAabbMaxLocal = convexFromLocal; + rayAabbMaxLocal.setMax(convexToLocal); + rayAabbMinLocal += boxMinLocal; + rayAabbMaxLocal += boxMaxLocal; + concaveShape->processAllTriangles(&tccb,rayAabbMinLocal,rayAabbMaxLocal); + } } } else { ///@todo : use AABB tree or other BVH acceleration structure! @@ -1162,15 +1198,14 @@ public: wv1 = m_worldTrans*triangle[1]; wv2 = m_worldTrans*triangle[2]; btVector3 center = (wv0+wv1+wv2)*btScalar(1./3.); - - btVector3 normal = (wv1-wv0).cross(wv2-wv0); - normal.normalize(); - btVector3 normalColor(1,1,0); - m_debugDrawer->drawLine(center,center+normal,normalColor); - - - - + + if (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawNormals ) + { + btVector3 normal = (wv1-wv0).cross(wv2-wv0); + normal.normalize(); + btVector3 normalColor(1,1,0); + m_debugDrawer->drawLine(center,center+normal,normalColor); + } m_debugDrawer->drawLine(wv0,wv1,m_color); m_debugDrawer->drawLine(wv1,wv2,m_color); m_debugDrawer->drawLine(wv2,wv0,m_color); @@ -1195,126 +1230,162 @@ void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const } else { - switch (shape->getShapeType()) - { - case BOX_SHAPE_PROXYTYPE: - { - const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape); - btVector3 halfExtents = boxShape->getHalfExtentsWithMargin(); - getDebugDrawer()->drawBox(-halfExtents,halfExtents,worldTransform,color); - break; - } + /// for polyhedral shapes + if (shape->isPolyhedral()) + { + btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape; - case SPHERE_SHAPE_PROXYTYPE: + int i; + if (polyshape->getConvexPolyhedron()) { - const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape); - btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin + const btConvexPolyhedron* poly = polyshape->getConvexPolyhedron(); + for (i=0;i<poly->m_faces.size();i++) + { + btVector3 centroid(0,0,0); + int numVerts = poly->m_faces[i].m_indices.size(); + if (numVerts) + { + int lastV = poly->m_faces[i].m_indices[numVerts-1]; + for (int v=0;v<poly->m_faces[i].m_indices.size();v++) + { + int curVert = poly->m_faces[i].m_indices[v]; + centroid+=poly->m_vertices[curVert]; + getDebugDrawer()->drawLine(worldTransform*poly->m_vertices[lastV],worldTransform*poly->m_vertices[curVert],color); + lastV = curVert; + } + } + centroid*= btScalar(1.f)/btScalar(numVerts); + if (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawNormals) + { + btVector3 normalColor(1,1,0); + btVector3 faceNormal(poly->m_faces[i].m_plane[0],poly->m_faces[i].m_plane[1],poly->m_faces[i].m_plane[2]); + getDebugDrawer()->drawLine(worldTransform*centroid,worldTransform*(centroid+faceNormal),normalColor); + } + + } - getDebugDrawer()->drawSphere(radius, worldTransform, color); - break; - } - case MULTI_SPHERE_SHAPE_PROXYTYPE: + + } else { - const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape); - - btTransform childTransform; - childTransform.setIdentity(); - - for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--) + for (i=0;i<polyshape->getNumEdges();i++) { - childTransform.setOrigin(multiSphereShape->getSpherePosition(i)); - getDebugDrawer()->drawSphere(multiSphereShape->getSphereRadius(i), worldTransform*childTransform, color); + btVector3 a,b; + polyshape->getEdge(i,a,b); + btVector3 wa = worldTransform * a; + btVector3 wb = worldTransform * b; + getDebugDrawer()->drawLine(wa,wb,color); } - - break; } - case CAPSULE_SHAPE_PROXYTYPE: - { - const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape); - btScalar radius = capsuleShape->getRadius(); - btScalar halfHeight = capsuleShape->getHalfHeight(); - int upAxis = capsuleShape->getUpAxis(); - getDebugDrawer()->drawCapsule(radius, halfHeight, upAxis, worldTransform, color); - break; - } - case CONE_SHAPE_PROXYTYPE: + } + else + { + switch (shape->getShapeType()) { - const btConeShape* coneShape = static_cast<const btConeShape*>(shape); - btScalar radius = coneShape->getRadius();//+coneShape->getMargin(); - btScalar height = coneShape->getHeight();//+coneShape->getMargin(); - int upAxis= coneShape->getConeUpIndex(); - getDebugDrawer()->drawCone(radius, height, upAxis, worldTransform, color); - break; + case BOX_SHAPE_PROXYTYPE: + { + const btBoxShape* boxShape = static_cast<const btBoxShape*>(shape); + btVector3 halfExtents = boxShape->getHalfExtentsWithMargin(); + getDebugDrawer()->drawBox(-halfExtents,halfExtents,worldTransform,color); + break; + } - } - case CYLINDER_SHAPE_PROXYTYPE: - { - const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape); - int upAxis = cylinder->getUpAxis(); - btScalar radius = cylinder->getRadius(); - btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis]; - getDebugDrawer()->drawCylinder(radius, halfHeight, upAxis, worldTransform, color); - break; - } + case SPHERE_SHAPE_PROXYTYPE: + { + const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape); + btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin - case STATIC_PLANE_PROXYTYPE: - { - const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape); - btScalar planeConst = staticPlaneShape->getPlaneConstant(); - const btVector3& planeNormal = staticPlaneShape->getPlaneNormal(); - getDebugDrawer()->drawPlane(planeNormal, planeConst,worldTransform, color); - break; + getDebugDrawer()->drawSphere(radius, worldTransform, color); + break; + } + case MULTI_SPHERE_SHAPE_PROXYTYPE: + { + const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape); - } - default: - { + btTransform childTransform; + childTransform.setIdentity(); - if (shape->isConcave()) - { - btConcaveShape* concaveMesh = (btConcaveShape*) shape; + for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--) + { + childTransform.setOrigin(multiSphereShape->getSpherePosition(i)); + getDebugDrawer()->drawSphere(multiSphereShape->getSphereRadius(i), worldTransform*childTransform, color); + } - ///@todo pass camera, for some culling? no -> we are not a graphics lib - btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); + break; + } + case CAPSULE_SHAPE_PROXYTYPE: + { + const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape); - DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color); - concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax); + btScalar radius = capsuleShape->getRadius(); + btScalar halfHeight = capsuleShape->getHalfHeight(); + int upAxis = capsuleShape->getUpAxis(); + getDebugDrawer()->drawCapsule(radius, halfHeight, upAxis, worldTransform, color); + break; } + case CONE_SHAPE_PROXYTYPE: + { + const btConeShape* coneShape = static_cast<const btConeShape*>(shape); + btScalar radius = coneShape->getRadius();//+coneShape->getMargin(); + btScalar height = coneShape->getHeight();//+coneShape->getMargin(); - if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE) + int upAxis= coneShape->getConeUpIndex(); + getDebugDrawer()->drawCone(radius, height, upAxis, worldTransform, color); + break; + + } + case CYLINDER_SHAPE_PROXYTYPE: { - btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape; - //todo: pass camera for some culling - btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); - btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); - //DebugDrawcallback drawCallback; - DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color); - convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax); + const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape); + int upAxis = cylinder->getUpAxis(); + btScalar radius = cylinder->getRadius(); + btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis]; + getDebugDrawer()->drawCylinder(radius, halfHeight, upAxis, worldTransform, color); + break; } + case STATIC_PLANE_PROXYTYPE: + { + const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape); + btScalar planeConst = staticPlaneShape->getPlaneConstant(); + const btVector3& planeNormal = staticPlaneShape->getPlaneNormal(); + getDebugDrawer()->drawPlane(planeNormal, planeConst,worldTransform, color); + break; - /// for polyhedral shapes - if (shape->isPolyhedral()) + } + default: { - btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape; - int i; - for (i=0;i<polyshape->getNumEdges();i++) + if (shape->isConcave()) { - btVector3 a,b; - polyshape->getEdge(i,a,b); - btVector3 wa = worldTransform * a; - btVector3 wb = worldTransform * b; - getDebugDrawer()->drawLine(wa,wb,color); + btConcaveShape* concaveMesh = (btConcaveShape*) shape; + + ///@todo pass camera, for some culling? no -> we are not a graphics lib + btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); + btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); + + DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color); + concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax); } + if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE) + { + btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape; + //todo: pass camera for some culling + btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); + btVector3 aabbMin(btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT),btScalar(-BT_LARGE_FLOAT)); + //DebugDrawcallback drawCallback; + DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color); + convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax); + } + + } } } @@ -1327,7 +1398,7 @@ void btCollisionWorld::debugDrawWorld() if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints) { int numManifolds = getDispatcher()->getNumManifolds(); - btVector3 color(0,0,0); + btVector3 color(1,0.65,0); for (int i=0;i<numManifolds;i++) { btPersistentManifold* contactManifold = getDispatcher()->getManifoldByIndexInternal(i); @@ -1343,7 +1414,7 @@ void btCollisionWorld::debugDrawWorld() } } - if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb)) + if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb))) { int i; @@ -1352,7 +1423,7 @@ void btCollisionWorld::debugDrawWorld() btCollisionObject* colObj = m_collisionObjects[i]; if ((colObj->getCollisionFlags() & btCollisionObject::CF_DISABLE_VISUALIZE_OBJECT)==0) { - if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe) + if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)) { btVector3 color(btScalar(1.),btScalar(1.),btScalar(1.)); switch(colObj->getActivationState()) @@ -1386,12 +1457,14 @@ void btCollisionWorld::debugDrawWorld() btVector3 minAabb2,maxAabb2; - colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(),minAabb2,maxAabb2); - minAabb2 -= contactThreshold; - maxAabb2 += contactThreshold; - - minAabb.setMin(minAabb2); - maxAabb.setMax(maxAabb2); + if(getDispatchInfo().m_useContinuous && colObj->getInternalType()==btCollisionObject::CO_RIGID_BODY && !colObj->isStaticOrKinematicObject()) + { + colObj->getCollisionShape()->getAabb(colObj->getInterpolationWorldTransform(),minAabb2,maxAabb2); + minAabb2 -= contactThreshold; + maxAabb2 += contactThreshold; + minAabb.setMin(minAabb2); + maxAabb.setMax(maxAabb2); + } m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec); } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h index b42e2c40b21..0a92d2d6e15 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCollisionWorld.h @@ -62,8 +62,8 @@ subject to the following restrictions: -#ifndef COLLISION_WORLD_H -#define COLLISION_WORLD_H +#ifndef BT_COLLISION_WORLD_H +#define BT_COLLISION_WORLD_H class btStackAlloc; class btCollisionShape; @@ -506,4 +506,4 @@ public: }; -#endif //COLLISION_WORLD_H +#endif //BT_COLLISION_WORLD_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h index 255e0af668c..404574989ab 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef COMPOUND_COLLISION_ALGORITHM_H -#define COMPOUND_COLLISION_ALGORITHM_H +#ifndef BT_COMPOUND_COLLISION_ALGORITHM_H +#define BT_COMPOUND_COLLISION_ALGORITHM_H #include "btActivatingCollisionAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btDispatcher.h" @@ -83,4 +83,4 @@ public: }; -#endif //COMPOUND_COLLISION_ALGORITHM_H +#endif //BT_COMPOUND_COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h index 5738401401e..53d13b87151 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvex2dConvex2dAlgorithm.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef CONVEX_2D_CONVEX_2D_ALGORITHM_H -#define CONVEX_2D_CONVEX_2D_ALGORITHM_H +#ifndef BT_CONVEX_2D_CONVEX_2D_ALGORITHM_H +#define BT_CONVEX_2D_CONVEX_2D_ALGORITHM_H #include "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h" #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" @@ -92,4 +92,4 @@ public: }; -#endif //CONVEX_2D_CONVEX_2D_ALGORITHM_H +#endif //BT_CONVEX_2D_CONVEX_2D_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp index 268ec4b6c7e..d2b2c221426 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.cpp @@ -91,7 +91,7 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, i btCollisionObject* ob = static_cast<btCollisionObject*>(m_triBody); - +#if 0 ///debug drawing of the overlapping triangles if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && (m_dispatchInfoPtr->m_debugDraw->getDebugMode() &btIDebugDraw::DBG_DrawWireframe )) { @@ -100,17 +100,8 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, i m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(triangle[1]),color); m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(triangle[2]),color); m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(triangle[0]),color); - - //btVector3 center = triangle[0] + triangle[1]+triangle[2]; - //center *= btScalar(0.333333); - //m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]),tr(center),color); - //m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]),tr(center),color); - //m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]),tr(center),color); - } - - - //btCollisionObject* colObj = static_cast<btCollisionObject*>(m_convexProxy->m_clientObject); +#endif if (m_convexBody->getCollisionShape()->isConvex()) { @@ -119,7 +110,7 @@ void btConvexTriangleCallback::processTriangle(btVector3* triangle,int partId, i btCollisionShape* tmpShape = ob->getCollisionShape(); ob->internalSetTemporaryCollisionShape( &tm ); - + btCollisionAlgorithm* colAlgo = ci.m_dispatcher1->findAlgorithm(m_convexBody,m_triBody,m_manifoldPtr); if (m_resultOut->getBody0Internal() == m_triBody) diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h index 984a4c39e8e..f718d1dec25 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConcaveCollisionAlgorithm.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef CONVEX_CONCAVE_COLLISION_ALGORITHM_H -#define CONVEX_CONCAVE_COLLISION_ALGORITHM_H +#ifndef BT_CONVEX_CONCAVE_COLLISION_ALGORITHM_H +#define BT_CONVEX_CONCAVE_COLLISION_ALGORITHM_H #include "btActivatingCollisionAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btDispatcher.h" @@ -113,4 +113,4 @@ public: }; -#endif //CONVEX_CONCAVE_COLLISION_ALGORITHM_H +#endif //BT_CONVEX_CONCAVE_COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp index 94385fed909..dd1f3e2490f 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp @@ -17,6 +17,7 @@ subject to the following restrictions: ///If you experience problems with capsule-capsule collision, try to define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER and report it in the Bullet forums ///with reproduction case //define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER 1 +//#define ZERO_MARGIN #include "btConvexConvexAlgorithm.h" @@ -26,6 +27,8 @@ subject to the following restrictions: #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/CollisionShapes/btConvexShape.h" #include "BulletCollision/CollisionShapes/btCapsuleShape.h" +#include "BulletCollision/CollisionShapes/btTriangleShape.h" + #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" @@ -48,7 +51,7 @@ subject to the following restrictions: #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" #include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h" - +#include "BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h" /////////// @@ -331,6 +334,8 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl #endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER + + #ifdef USE_SEPDISTANCE_UTIL2 if (dispatchInfo.m_useConvexConservativeDistanceUtil) { @@ -357,13 +362,14 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl } else #endif //USE_SEPDISTANCE_UTIL2 { - if (dispatchInfo.m_convexMaxDistanceUseCPT) - { - input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactProcessingThreshold(); - } else - { - input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold(); - } + //if (dispatchInfo.m_convexMaxDistanceUseCPT) + //{ + // input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactProcessingThreshold(); + //} else + //{ + input.m_maximumDistanceSquared = min0->getMargin() + min1->getMargin() + m_manifoldPtr->getContactBreakingThreshold(); +// } + input.m_maximumDistanceSquared*= input.m_maximumDistanceSquared; } @@ -371,7 +377,7 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl input.m_transformA = body0->getWorldTransform(); input.m_transformB = body1->getWorldTransform(); - gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); + @@ -389,6 +395,155 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl } #endif //USE_SEPDISTANCE_UTIL2 + if (min0->isPolyhedral() && min1->isPolyhedral()) + { + + + struct btDummyResult : public btDiscreteCollisionDetectorInterface::Result + { + virtual void setShapeIdentifiersA(int partId0,int index0){} + virtual void setShapeIdentifiersB(int partId1,int index1){} + virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth) + { + } + }; + + btDummyResult dummy; + + + btPolyhedralConvexShape* polyhedronA = (btPolyhedralConvexShape*) min0; + btPolyhedralConvexShape* polyhedronB = (btPolyhedralConvexShape*) min1; + if (polyhedronA->getConvexPolyhedron() && polyhedronB->getConvexPolyhedron()) + { + + + + + btScalar threshold = m_manifoldPtr->getContactBreakingThreshold(); + + btScalar minDist = -1e30f; + btVector3 sepNormalWorldSpace; + bool foundSepAxis = true; + + if (dispatchInfo.m_enableSatConvex) + { + foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis( + *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), + body0->getWorldTransform(), + body1->getWorldTransform(), + sepNormalWorldSpace); + } else + { +#ifdef ZERO_MARGIN + gjkPairDetector.setIgnoreMargin(true); + gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); +#else + //gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); + gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw); +#endif //ZERO_MARGIN + btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); + if (l2>SIMD_EPSILON) + { + sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2); + //minDist = -1e30f;//gjkPairDetector.getCachedSeparatingDistance(); + minDist = gjkPairDetector.getCachedSeparatingDistance()-min0->getMargin()-min1->getMargin(); + +#ifdef ZERO_MARGIN + foundSepAxis = true;//gjkPairDetector.getCachedSeparatingDistance()<0.f; +#else + foundSepAxis = gjkPairDetector.getCachedSeparatingDistance()<(min0->getMargin()+min1->getMargin()); +#endif + } + } + if (foundSepAxis) + { +// printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ()); + + btPolyhedralContactClipping::clipHullAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), + body0->getWorldTransform(), + body1->getWorldTransform(), minDist-threshold, threshold, *resultOut); + + } + if (m_ownManifold) + { + resultOut->refreshContactPoints(); + } + return; + + } else + { + //we can also deal with convex versus triangle (without connectivity data) + if (polyhedronA->getConvexPolyhedron() && polyhedronB->getShapeType()==TRIANGLE_SHAPE_PROXYTYPE) + { + + btVertexArray vertices; + btTriangleShape* tri = (btTriangleShape*)polyhedronB; + vertices.push_back( body1->getWorldTransform()*tri->m_vertices1[0]); + vertices.push_back( body1->getWorldTransform()*tri->m_vertices1[1]); + vertices.push_back( body1->getWorldTransform()*tri->m_vertices1[2]); + + //tri->initializePolyhedralFeatures(); + + btScalar threshold = m_manifoldPtr->getContactBreakingThreshold(); + + btVector3 sepNormalWorldSpace; + btScalar minDist =-1e30f; + btScalar maxDist = threshold; + + bool foundSepAxis = false; + if (0) + { + polyhedronB->initializePolyhedralFeatures(); + foundSepAxis = btPolyhedralContactClipping::findSeparatingAxis( + *polyhedronA->getConvexPolyhedron(), *polyhedronB->getConvexPolyhedron(), + body0->getWorldTransform(), + body1->getWorldTransform(), + sepNormalWorldSpace); + // printf("sepNormalWorldSpace=%f,%f,%f\n",sepNormalWorldSpace.getX(),sepNormalWorldSpace.getY(),sepNormalWorldSpace.getZ()); + + } else + { +#ifdef ZERO_MARGIN + gjkPairDetector.setIgnoreMargin(true); + gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); +#else + gjkPairDetector.getClosestPoints(input,dummy,dispatchInfo.m_debugDraw); +#endif//ZERO_MARGIN + + btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); + if (l2>SIMD_EPSILON) + { + sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2); + //minDist = gjkPairDetector.getCachedSeparatingDistance(); + //maxDist = threshold; + minDist = gjkPairDetector.getCachedSeparatingDistance()-min0->getMargin()-min1->getMargin(); + foundSepAxis = true; + } + } + + + if (foundSepAxis) + { + btPolyhedralContactClipping::clipFaceAgainstHull(sepNormalWorldSpace, *polyhedronA->getConvexPolyhedron(), + body0->getWorldTransform(), vertices, minDist-threshold, maxDist, *resultOut); + } + + + if (m_ownManifold) + { + resultOut->refreshContactPoints(); + } + + return; + } + + } + + + } + + gjkPairDetector.getClosestPoints(input,*resultOut,dispatchInfo.m_debugDraw); + //now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects //perform perturbation when more then 'm_minimumPointsPerturbationThreshold' points @@ -398,66 +553,70 @@ void btConvexConvexAlgorithm ::processCollision (btCollisionObject* body0,btColl int i; btVector3 v0,v1; btVector3 sepNormalWorldSpace; + btScalar l2 = gjkPairDetector.getCachedSeparatingAxis().length2(); - sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis().normalized(); - btPlaneSpace1(sepNormalWorldSpace,v0,v1); - - - bool perturbeA = true; - const btScalar angleLimit = 0.125f * SIMD_PI; - btScalar perturbeAngle; - btScalar radiusA = min0->getAngularMotionDisc(); - btScalar radiusB = min1->getAngularMotionDisc(); - if (radiusA < radiusB) - { - perturbeAngle = gContactBreakingThreshold /radiusA; - perturbeA = true; - } else + if (l2>SIMD_EPSILON) { - perturbeAngle = gContactBreakingThreshold / radiusB; - perturbeA = false; - } - if ( perturbeAngle > angleLimit ) - perturbeAngle = angleLimit; + sepNormalWorldSpace = gjkPairDetector.getCachedSeparatingAxis()*(1.f/l2); + + btPlaneSpace1(sepNormalWorldSpace,v0,v1); - btTransform unPerturbedTransform; - if (perturbeA) - { - unPerturbedTransform = input.m_transformA; - } else - { - unPerturbedTransform = input.m_transformB; - } - - for ( i=0;i<m_numPerturbationIterations;i++) - { - if (v0.length2()>SIMD_EPSILON) + + bool perturbeA = true; + const btScalar angleLimit = 0.125f * SIMD_PI; + btScalar perturbeAngle; + btScalar radiusA = min0->getAngularMotionDisc(); + btScalar radiusB = min1->getAngularMotionDisc(); + if (radiusA < radiusB) { - btQuaternion perturbeRot(v0,perturbeAngle); - btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations)); - btQuaternion rotq(sepNormalWorldSpace,iterationAngle); - - + perturbeAngle = gContactBreakingThreshold /radiusA; + perturbeA = true; + } else + { + perturbeAngle = gContactBreakingThreshold / radiusB; + perturbeA = false; + } + if ( perturbeAngle > angleLimit ) + perturbeAngle = angleLimit; + + btTransform unPerturbedTransform; if (perturbeA) { - input.m_transformA.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body0->getWorldTransform().getBasis()); - input.m_transformB = body1->getWorldTransform(); -#ifdef DEBUG_CONTACTS - dispatchInfo.m_debugDraw->drawTransform(input.m_transformA,10.0); -#endif //DEBUG_CONTACTS + unPerturbedTransform = input.m_transformA; } else { - input.m_transformA = body0->getWorldTransform(); - input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1->getWorldTransform().getBasis()); -#ifdef DEBUG_CONTACTS - dispatchInfo.m_debugDraw->drawTransform(input.m_transformB,10.0); -#endif + unPerturbedTransform = input.m_transformB; } - btPerturbedContactResult perturbedResultOut(resultOut,input.m_transformA,input.m_transformB,unPerturbedTransform,perturbeA,dispatchInfo.m_debugDraw); - gjkPairDetector.getClosestPoints(input,perturbedResultOut,dispatchInfo.m_debugDraw); + for ( i=0;i<m_numPerturbationIterations;i++) + { + if (v0.length2()>SIMD_EPSILON) + { + btQuaternion perturbeRot(v0,perturbeAngle); + btScalar iterationAngle = i*(SIMD_2_PI/btScalar(m_numPerturbationIterations)); + btQuaternion rotq(sepNormalWorldSpace,iterationAngle); + + + if (perturbeA) + { + input.m_transformA.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body0->getWorldTransform().getBasis()); + input.m_transformB = body1->getWorldTransform(); + #ifdef DEBUG_CONTACTS + dispatchInfo.m_debugDraw->drawTransform(input.m_transformA,10.0); + #endif //DEBUG_CONTACTS + } else + { + input.m_transformA = body0->getWorldTransform(); + input.m_transformB.setBasis( btMatrix3x3(rotq.inverse()*perturbeRot*rotq)*body1->getWorldTransform().getBasis()); + #ifdef DEBUG_CONTACTS + dispatchInfo.m_debugDraw->drawTransform(input.m_transformB,10.0); + #endif + } + + btPerturbedContactResult perturbedResultOut(resultOut,input.m_transformA,input.m_transformB,unPerturbedTransform,perturbeA,dispatchInfo.m_debugDraw); + gjkPairDetector.getClosestPoints(input,perturbedResultOut,dispatchInfo.m_debugDraw); + } } - } } @@ -487,7 +646,7 @@ btScalar btConvexConvexAlgorithm::calculateTimeOfImpact(btCollisionObject* col0, { (void)resultOut; (void)dispatchInfo; - ///rather than checking ALL pairs, only calculate TOI when motion exceeds threshold + ///Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold ///Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold ///col0->m_worldTransform, diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h index d38aff6862c..4380b80eb4d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef CONVEX_CONVEX_ALGORITHM_H -#define CONVEX_CONVEX_ALGORITHM_H +#ifndef BT_CONVEX_CONVEX_ALGORITHM_H +#define BT_CONVEX_CONVEX_ALGORITHM_H #include "btActivatingCollisionAlgorithm.h" #include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h" @@ -106,4 +106,4 @@ public: }; -#endif //CONVEX_CONVEX_ALGORITHM_H +#endif //BT_CONVEX_CONVEX_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp index dda85dc693f..b2e9bfaf593 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.cpp @@ -96,23 +96,41 @@ void btConvexPlaneCollisionAlgorithm::processCollision (btCollisionObject* body0 if (!m_manifoldPtr) return; - btCollisionObject* convexObj = m_isSwapped? body1 : body0; + btCollisionObject* convexObj = m_isSwapped? body1 : body0; btCollisionObject* planeObj = m_isSwapped? body0: body1; btConvexShape* convexShape = (btConvexShape*) convexObj->getCollisionShape(); btStaticPlaneShape* planeShape = (btStaticPlaneShape*) planeObj->getCollisionShape(); - + bool hasCollision = false; const btVector3& planeNormal = planeShape->getPlaneNormal(); - //const btScalar& planeConstant = planeShape->getPlaneConstant(); + const btScalar& planeConstant = planeShape->getPlaneConstant(); + btTransform planeInConvex; + planeInConvex= convexObj->getWorldTransform().inverse() * planeObj->getWorldTransform(); + btTransform convexInPlaneTrans; + convexInPlaneTrans= planeObj->getWorldTransform().inverse() * convexObj->getWorldTransform(); + + btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal); + btVector3 vtxInPlane = convexInPlaneTrans(vtx); + btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant); + + btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal; + btVector3 vtxInPlaneWorld = planeObj->getWorldTransform() * vtxInPlaneProjected; - //first perform a collision query with the non-perturbated collision objects + hasCollision = distance < m_manifoldPtr->getContactBreakingThreshold(); + resultOut->setPersistentManifold(m_manifoldPtr); + if (hasCollision) { - btQuaternion rotq(0,0,0,1); - collideSingleContact(rotq,body0,body1,dispatchInfo,resultOut); + /// report a contact. internally this will be kept persistent, and contact reduction is done + btVector3 normalOnSurfaceB = planeObj->getWorldTransform().getBasis() * planeNormal; + btVector3 pOnB = vtxInPlaneWorld; + resultOut->addContactPoint(normalOnSurfaceB,pOnB,distance); } - if (resultOut->getPersistentManifold()->getNumContacts()<m_minimumPointsPerturbationThreshold) + //the perturbation algorithm doesn't work well with implicit surfaces such as spheres, cylinder and cones: + //they keep on rolling forever because of the additional off-center contact points + //so only enable the feature for polyhedral shapes (btBoxShape, btConvexHullShape etc) + if (convexShape->isPolyhedral() && resultOut->getPersistentManifold()->getNumContacts()<m_minimumPointsPerturbationThreshold) { btVector3 v0,v1; btPlaneSpace1(planeNormal,v0,v1); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h index f49ac45e772..b9494f5ad3b 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btConvexPlaneCollisionAlgorithm.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef CONVEX_PLANE_COLLISION_ALGORITHM_H -#define CONVEX_PLANE_COLLISION_ALGORITHM_H +#ifndef BT_CONVEX_PLANE_COLLISION_ALGORITHM_H +#define BT_CONVEX_PLANE_COLLISION_ALGORITHM_H #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" @@ -61,7 +61,7 @@ public: CreateFunc() : m_numPerturbationIterations(1), - m_minimumPointsPerturbationThreshold(1) + m_minimumPointsPerturbationThreshold(0) { } @@ -80,5 +80,5 @@ public: }; -#endif //CONVEX_PLANE_COLLISION_ALGORITHM_H +#endif //BT_CONVEX_PLANE_COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp index c27d8ce0752..7faee6faf50 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.cpp @@ -296,3 +296,14 @@ void btDefaultCollisionConfiguration::setConvexConvexMultipointIterations(int nu convexConvex->m_numPerturbationIterations = numPerturbationIterations; convexConvex->m_minimumPointsPerturbationThreshold = minimumPointsPerturbationThreshold; } + +void btDefaultCollisionConfiguration::setPlaneConvexMultipointIterations(int numPerturbationIterations, int minimumPointsPerturbationThreshold) +{ + btConvexPlaneCollisionAlgorithm::CreateFunc* cpCF = (btConvexPlaneCollisionAlgorithm::CreateFunc*)m_convexPlaneCF; + cpCF->m_numPerturbationIterations = numPerturbationIterations; + cpCF->m_minimumPointsPerturbationThreshold = minimumPointsPerturbationThreshold; + + btConvexPlaneCollisionAlgorithm::CreateFunc* pcCF = (btConvexPlaneCollisionAlgorithm::CreateFunc*)m_planeConvexCF; + pcCF->m_numPerturbationIterations = numPerturbationIterations; + pcCF->m_minimumPointsPerturbationThreshold = minimumPointsPerturbationThreshold; +} diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h index 6aa0d8c270f..81ed424a3db 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h @@ -129,6 +129,8 @@ public: ///@todo we could add a per-object setting of those parameters, for level-of-detail collision detection. void setConvexConvexMultipointIterations(int numPerturbationIterations=3, int minimumPointsPerturbationThreshold = 3); + void setPlaneConvexMultipointIterations(int numPerturbationIterations=3, int minimumPointsPerturbationThreshold = 3); + }; #endif //BT_DEFAULT_COLLISION_CONFIGURATION diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h index e54721dec21..f03c9dc3833 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btEmptyCollisionAlgorithm.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef EMPTY_ALGORITH -#define EMPTY_ALGORITH +#ifndef BT_EMPTY_ALGORITH +#define BT_EMPTY_ALGORITH #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" #include "btCollisionCreateFunc.h" #include "btCollisionDispatcher.h" @@ -51,4 +51,4 @@ public: } ATTRIBUTE_ALIGNED(16); -#endif //EMPTY_ALGORITH +#endif //BT_EMPTY_ALGORITH diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp index 5cceb04dbb4..4353cdac0b1 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btInternalEdgeUtility.cpp @@ -1,6 +1,7 @@ #include "btInternalEdgeUtility.h" #include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" +#include "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h" #include "BulletCollision/CollisionShapes/btTriangleShape.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h" @@ -9,7 +10,6 @@ //#define DEBUG_INTERNAL_EDGE - #ifdef DEBUG_INTERNAL_EDGE #include <stdio.h> #endif //DEBUG_INTERNAL_EDGE @@ -456,8 +456,14 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject* if (colObj0->getCollisionShape()->getShapeType() != TRIANGLE_SHAPE_PROXYTYPE) return; - btBvhTriangleMeshShape* trimesh = (btBvhTriangleMeshShape*)colObj0->getRootCollisionShape(); - btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*) trimesh->getTriangleInfoMap(); + btBvhTriangleMeshShape* trimesh = 0; + + if( colObj0->getRootCollisionShape()->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE ) + trimesh = ((btScaledBvhTriangleMeshShape*)colObj0->getRootCollisionShape())->getChildShape(); + else + trimesh = (btBvhTriangleMeshShape*)colObj0->getRootCollisionShape(); + + btTriangleInfoMap* triangleInfoMapPtr = (btTriangleInfoMap*) trimesh->getTriangleInfoMap(); if (!triangleInfoMapPtr) return; @@ -501,14 +507,63 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject* btVector3 localContactNormalOnB = colObj0->getWorldTransform().getBasis().transpose() * cp.m_normalWorldOnB; localContactNormalOnB.normalize();//is this necessary? - - if ((info->m_edgeV0V1Angle)< SIMD_2_PI) + + // Get closest edge + int bestedge=-1; + btScalar disttobestedge=BT_LARGE_FLOAT; + // + // Edge 0 -> 1 + if (btFabs(info->m_edgeV0V1Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) + { + btVector3 nearest; + btNearestPointInLineSegment( cp.m_localPointB, v0, v1, nearest ); + btScalar len=(contact-nearest).length(); + // + if( len < disttobestedge ) + { + bestedge=0; + disttobestedge=len; + } + } + // Edge 1 -> 2 + if (btFabs(info->m_edgeV1V2Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) + { + btVector3 nearest; + btNearestPointInLineSegment( cp.m_localPointB, v1, v2, nearest ); + btScalar len=(contact-nearest).length(); + // + if( len < disttobestedge ) + { + bestedge=1; + disttobestedge=len; + } + } + // Edge 2 -> 0 + if (btFabs(info->m_edgeV2V0Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) + { + btVector3 nearest; + btNearestPointInLineSegment( cp.m_localPointB, v2, v0, nearest ); + btScalar len=(contact-nearest).length(); + // + if( len < disttobestedge ) + { + bestedge=2; + disttobestedge=len; + } + } + +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btVector3 upfix=tri_normal * btVector3(0.1f,0.1f,0.1f); + btDebugDrawLine(tr * v0 + upfix, tr * v1 + upfix, red ); +#endif + if (btFabs(info->m_edgeV0V1Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) { #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black); #endif btScalar len = (contact-nearest).length(); if(len<triangleInfoMapPtr->m_edgeDistanceThreshold) + if( bestedge==0 ) { btVector3 edge(v0-v1); isNearEdge = true; @@ -577,7 +632,11 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject* btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,green); #endif //BT_INTERNAL_EDGE_DEBUG_DRAW - if ((info->m_edgeV1V2Angle)< SIMD_2_PI) +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr * v1 + upfix, tr * v2 + upfix , green ); +#endif + + if (btFabs(info->m_edgeV1V2Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) { #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW btDebugDrawLine(tr*contact,tr*(contact+cp.m_normalWorldOnB*10),black); @@ -587,6 +646,7 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject* btScalar len = (contact-nearest).length(); if(len<triangleInfoMapPtr->m_edgeDistanceThreshold) + if( bestedge==1 ) { isNearEdge = true; #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW @@ -658,8 +718,11 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject* #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW btDebugDrawLine(tr*nearest,tr*cp.m_localPointB,blue); #endif //BT_INTERNAL_EDGE_DEBUG_DRAW +#ifdef BT_INTERNAL_EDGE_DEBUG_DRAW + btDebugDrawLine(tr * v2 + upfix, tr * v0 + upfix , blue ); +#endif - if ((info->m_edgeV2V0Angle)< SIMD_2_PI) + if (btFabs(info->m_edgeV2V0Angle)< triangleInfoMapPtr->m_maxEdgeAngleThreshold) { #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW @@ -668,6 +731,7 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject* btScalar len = (contact-nearest).length(); if(len<triangleInfoMapPtr->m_edgeDistanceThreshold) + if( bestedge==2 ) { isNearEdge = true; #ifdef BT_INTERNAL_EDGE_DEBUG_DRAW @@ -759,11 +823,17 @@ void btAdjustInternalEdgeContacts(btManifoldPoint& cp, const btCollisionObject* cp.m_normalWorldOnB = colObj0->getWorldTransform().getBasis()*tri_normal; } else { + btVector3 newNormal = tri_normal *frontFacing; + //if the tri_normal is pointing opposite direction as the current local contact normal, skip it + btScalar d = newNormal.dot(localContactNormalOnB) ; + if (d< 0) + { + return; + } //modify the normal to be the triangle normal (or backfacing normal) - cp.m_normalWorldOnB = colObj0->getWorldTransform().getBasis() *(tri_normal *frontFacing); + cp.m_normalWorldOnB = colObj0->getWorldTransform().getBasis() *newNormal; } - - + // Reproject collision point along normal. cp.m_positionWorldOnB = cp.m_positionWorldOnA - cp.m_normalWorldOnB * cp.m_distance1; cp.m_localPointB = colObj0->getWorldTransform().invXform(cp.m_positionWorldOnB); diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp index fd684c056f7..bf24246ea2f 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp @@ -64,8 +64,8 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b btAssert(m_manifoldPtr); //order in manifold needs to match -// if (depth > m_manifoldPtr->getContactBreakingThreshold()) - if (depth > m_manifoldPtr->getContactProcessingThreshold()) + if (depth > m_manifoldPtr->getContactBreakingThreshold()) +// if (depth > m_manifoldPtr->getContactProcessingThreshold()) return; bool isSwapped = m_manifoldPtr->getBody0() != m_body0; diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h index 927e2bc4f76..18199b49752 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btManifoldResult.h @@ -14,8 +14,8 @@ subject to the following restrictions: */ -#ifndef MANIFOLD_RESULT_H -#define MANIFOLD_RESULT_H +#ifndef BT_MANIFOLD_RESULT_H +#define BT_MANIFOLD_RESULT_H class btCollisionObject; #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" @@ -125,4 +125,4 @@ public: }; -#endif //MANIFOLD_RESULT_H +#endif //BT_MANIFOLD_RESULT_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp index bb2a7f23985..871c64415a2 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.cpp @@ -47,6 +47,8 @@ void btSimulationIslandManager::findUnions(btDispatcher* /* dispatcher */,btColl { btOverlappingPairCache* pairCachePtr = colWorld->getPairCache(); const int numOverlappingPairs = pairCachePtr->getNumOverlappingPairs(); + if (numOverlappingPairs) + { btBroadphasePair* pairPtr = pairCachePtr->getOverlappingPairArrayPtr(); for (int i=0;i<numOverlappingPairs;i++) @@ -63,6 +65,7 @@ void btSimulationIslandManager::findUnions(btDispatcher* /* dispatcher */,btColl (colObj1)->getIslandTag()); } } + } } } @@ -190,7 +193,7 @@ class btPersistentManifoldSortPredicate { public: - SIMD_FORCE_INLINE bool operator() ( const btPersistentManifold* lhs, const btPersistentManifold* rhs ) + SIMD_FORCE_INLINE bool operator() ( const btPersistentManifold* lhs, const btPersistentManifold* rhs ) const { return getIslandId(lhs) < getIslandId(rhs); } @@ -327,11 +330,13 @@ void btSimulationIslandManager::buildIslands(btDispatcher* dispatcher,btCollisio //kinematic objects don't merge islands, but wake up all connected objects if (colObj0->isKinematicObject() && colObj0->getActivationState() != ISLAND_SLEEPING) { - colObj1->activate(); + if (colObj0->hasContactResponse()) + colObj1->activate(); } if (colObj1->isKinematicObject() && colObj1->getActivationState() != ISLAND_SLEEPING) { - colObj0->activate(); + if (colObj1->hasContactResponse()) + colObj0->activate(); } if(m_splitIslands) { @@ -362,7 +367,7 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, { btPersistentManifold** manifold = dispatcher->getInternalManifoldPointer(); int maxNumManifolds = dispatcher->getNumManifolds(); - callback->ProcessIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1); + callback->processIsland(&collisionObjects[0],collisionObjects.size(),manifold,maxNumManifolds, -1); } else { @@ -372,8 +377,10 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, int numManifolds = int (m_islandmanifold.size()); - //we should do radix sort, it it much faster (O(n) instead of O (n log2(n)) + //tried a radix sort, but quicksort/heapsort seems still faster + //@todo rewrite island management m_islandmanifold.quickSort(btPersistentManifoldSortPredicate()); + //m_islandmanifold.heapSort(btPersistentManifoldSortPredicate()); //now process all active islands (sets of manifolds for now) @@ -427,7 +434,7 @@ void btSimulationIslandManager::buildAndProcessIslands(btDispatcher* dispatcher, if (!islandSleeping) { - callback->ProcessIsland(&m_islandBodies[0],m_islandBodies.size(),startManifold,numIslandManifolds, islandId); + callback->processIsland(&m_islandBodies[0],m_islandBodies.size(),startManifold,numIslandManifolds, islandId); // printf("Island callback of size:%d bodies, %d manifolds\n",islandBodies.size(),numIslandManifolds); } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h index d059f5d6b0d..e24c6afeca1 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSimulationIslandManager.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef SIMULATION_ISLAND_MANAGER_H -#define SIMULATION_ISLAND_MANAGER_H +#ifndef BT_SIMULATION_ISLAND_MANAGER_H +#define BT_SIMULATION_ISLAND_MANAGER_H #include "BulletCollision/CollisionDispatch/btUnionFind.h" #include "btCollisionCreateFunc.h" @@ -59,7 +59,7 @@ public: { virtual ~IslandCallback() {}; - virtual void ProcessIsland(btCollisionObject** bodies,int numBodies,class btPersistentManifold** manifolds,int numManifolds, int islandId) = 0; + virtual void processIsland(btCollisionObject** bodies,int numBodies,class btPersistentManifold** manifolds,int numManifolds, int islandId) = 0; }; void buildAndProcessIslands(btDispatcher* dispatcher,btCollisionWorld* collisionWorld, IslandCallback* callback); @@ -77,5 +77,5 @@ public: }; -#endif //SIMULATION_ISLAND_MANAGER_H +#endif //BT_SIMULATION_ISLAND_MANAGER_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h index 47111d1c4af..60286ae0aa4 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereBoxCollisionAlgorithm.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef SPHERE_BOX_COLLISION_ALGORITHM_H -#define SPHERE_BOX_COLLISION_ALGORITHM_H +#ifndef BT_SPHERE_BOX_COLLISION_ALGORITHM_H +#define BT_SPHERE_BOX_COLLISION_ALGORITHM_H #include "btActivatingCollisionAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" @@ -71,5 +71,5 @@ public: }; -#endif //SPHERE_BOX_COLLISION_ALGORITHM_H +#endif //BT_SPHERE_BOX_COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h index 7d07512ca66..e55acf277e6 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef SPHERE_SPHERE_COLLISION_ALGORITHM_H -#define SPHERE_SPHERE_COLLISION_ALGORITHM_H +#ifndef BT_SPHERE_SPHERE_COLLISION_ALGORITHM_H +#define BT_SPHERE_SPHERE_COLLISION_ALGORITHM_H #include "btActivatingCollisionAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" @@ -62,5 +62,5 @@ public: }; -#endif //SPHERE_SPHERE_COLLISION_ALGORITHM_H +#endif //BT_SPHERE_SPHERE_COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h index 606c3635ae9..7c6c4d8f8d5 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef SPHERE_TRIANGLE_COLLISION_ALGORITHM_H -#define SPHERE_TRIANGLE_COLLISION_ALGORITHM_H +#ifndef BT_SPHERE_TRIANGLE_COLLISION_ALGORITHM_H +#define BT_SPHERE_TRIANGLE_COLLISION_ALGORITHM_H #include "btActivatingCollisionAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" @@ -65,5 +65,5 @@ public: }; -#endif //SPHERE_TRIANGLE_COLLISION_ALGORITHM_H +#endif //BT_SPHERE_TRIANGLE_COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp index 4c4f58d44fa..5222933595d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.cpp @@ -53,7 +53,7 @@ class btUnionFindElementSortPredicate { public: - bool operator() ( const btElement& lhs, const btElement& rhs ) + bool operator() ( const btElement& lhs, const btElement& rhs ) const { return lhs.m_id < rhs.m_id; } diff --git a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h index 2cce335145b..ef2a29202f7 100644 --- a/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h +++ b/extern/bullet2/src/BulletCollision/CollisionDispatch/btUnionFind.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef UNION_FIND_H -#define UNION_FIND_H +#ifndef BT_UNION_FIND_H +#define BT_UNION_FIND_H #include "LinearMath/btAlignedObjectArray.h" @@ -126,4 +126,4 @@ class btUnionFind }; -#endif //UNION_FIND_H +#endif //BT_UNION_FIND_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBox2dShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btBox2dShape.h index fc032069c03..f4a9ca03e5c 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBox2dShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBox2dShape.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef OBB_BOX_2D_SHAPE_H -#define OBB_BOX_2D_SHAPE_H +#ifndef BT_OBB_BOX_2D_SHAPE_H +#define BT_OBB_BOX_2D_SHAPE_H #include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h" #include "BulletCollision/CollisionShapes/btCollisionMargin.h" @@ -83,6 +83,7 @@ public: } + ///a btBox2dShape is a flat 2D box in the X-Y plane (Z extents are zero) btBox2dShape( const btVector3& boxHalfExtents) : btPolyhedralConvexShape(), m_centroid(0,0,0) @@ -97,6 +98,11 @@ public: m_normals[2].setValue(0,1,0); m_normals[3].setValue(-1,0,0); + btScalar minDimension = boxHalfExtents.getX(); + if (minDimension>boxHalfExtents.getY()) + minDimension = boxHalfExtents.getY(); + setSafeMargin(minDimension); + m_shapeType = BOX_2D_SHAPE_PROXYTYPE; btVector3 margin(getMargin(),getMargin(),getMargin()); m_implicitShapeDimensions = (boxHalfExtents * m_localScaling) - margin; @@ -358,6 +364,6 @@ public: }; -#endif //OBB_BOX_2D_SHAPE_H +#endif //BT_OBB_BOX_2D_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.cpp index c6644efbef3..3859138f18a 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.cpp @@ -14,8 +14,18 @@ subject to the following restrictions: */ #include "btBoxShape.h" +btBoxShape::btBoxShape( const btVector3& boxHalfExtents) +: btPolyhedralConvexShape() +{ + m_shapeType = BOX_SHAPE_PROXYTYPE; + + setSafeMargin(boxHalfExtents); + + btVector3 margin(getMargin(),getMargin(),getMargin()); + m_implicitShapeDimensions = (boxHalfExtents * m_localScaling) - margin; +}; + -//{ void btBoxShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h index b405efc8e3c..0c5857dae62 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBoxShape.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef OBB_BOX_MINKOWSKI_H -#define OBB_BOX_MINKOWSKI_H +#ifndef BT_OBB_BOX_MINKOWSKI_H +#define BT_OBB_BOX_MINKOWSKI_H #include "btPolyhedralConvexShape.h" #include "btCollisionMargin.h" @@ -80,13 +80,7 @@ public: } - btBoxShape( const btVector3& boxHalfExtents) - : btPolyhedralConvexShape() - { - m_shapeType = BOX_SHAPE_PROXYTYPE; - btVector3 margin(getMargin(),getMargin(),getMargin()); - m_implicitShapeDimensions = (boxHalfExtents * m_localScaling) - margin; - }; + btBoxShape( const btVector3& boxHalfExtents); virtual void setMargin(btScalar collisionMargin) { @@ -145,7 +139,7 @@ public: virtual void getVertex(int i,btVector3& vtx) const { - btVector3 halfExtents = getHalfExtentsWithoutMargin(); + btVector3 halfExtents = getHalfExtentsWithMargin(); vtx = btVector3( halfExtents.x() * (1-(i&1)) - halfExtents.x() * (i&1), @@ -313,6 +307,6 @@ public: }; -#endif //OBB_BOX_MINKOWSKI_H +#endif //BT_OBB_BOX_MINKOWSKI_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h index c269ef27bdb..d1c21629873 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef BVH_TRIANGLE_MESH_SHAPE_H -#define BVH_TRIANGLE_MESH_SHAPE_H +#ifndef BT_BVH_TRIANGLE_MESH_SHAPE_H +#define BT_BVH_TRIANGLE_MESH_SHAPE_H #include "btTriangleMeshShape.h" #include "btOptimizedBvh.h" @@ -136,4 +136,4 @@ SIMD_FORCE_INLINE int btBvhTriangleMeshShape::calculateSerializeBufferSize() con -#endif //BVH_TRIANGLE_MESH_SHAPE_H +#endif //BT_BVH_TRIANGLE_MESH_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp index 2faa11d4360..864df26e931 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCapsuleShape.cpp @@ -55,7 +55,7 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInter btVector3 pos(0,0,0); pos[getUpAxis()] = getHalfHeight(); - vtx = pos +vec*m_localScaling*(radius) - vec * getMargin(); + vtx = pos +vec*(radius) - vec * getMargin(); newDot = vec.dot(vtx); if (newDot > maxDot) { @@ -67,7 +67,7 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInter btVector3 pos(0,0,0); pos[getUpAxis()] = -getHalfHeight(); - vtx = pos +vec*m_localScaling*(radius) - vec * getMargin(); + vtx = pos +vec*(radius) - vec * getMargin(); newDot = vec.dot(vtx); if (newDot > maxDot) { @@ -96,7 +96,7 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInter { btVector3 pos(0,0,0); pos[getUpAxis()] = getHalfHeight(); - vtx = pos +vec*m_localScaling*(radius) - vec * getMargin(); + vtx = pos +vec*(radius) - vec * getMargin(); newDot = vec.dot(vtx); if (newDot > maxDot) { @@ -107,7 +107,7 @@ btCapsuleShape::btCapsuleShape(btScalar radius, btScalar height) : btConvexInter { btVector3 pos(0,0,0); pos[getUpAxis()] = -getHalfHeight(); - vtx = pos +vec*m_localScaling*(radius) - vec * getMargin(); + vtx = pos +vec*(radius) - vec * getMargin(); newDot = vec.dot(vtx); if (newDot > maxDot) { diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionMargin.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionMargin.h index 18fd026041f..474bf1fb499 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionMargin.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionMargin.h @@ -13,14 +13,15 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef COLLISION_MARGIN_H -#define COLLISION_MARGIN_H - -//used by Gjk and some other algorithms +#ifndef BT_COLLISION_MARGIN_H +#define BT_COLLISION_MARGIN_H +///The CONVEX_DISTANCE_MARGIN is a default collision margin for convex collision shapes derived from btConvexInternalShape. +///This collision margin is used by Gjk and some other algorithms +///Note that when creating small objects, you need to make sure to set a smaller collision margin, using the 'setMargin' API #define CONVEX_DISTANCE_MARGIN btScalar(0.04)// btScalar(0.1)//;//btScalar(0.01) -#endif //COLLISION_MARGIN_H +#endif //BT_COLLISION_MARGIN_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h index f32bd736a99..865c1067744 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCollisionShape.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef COLLISION_SHAPE_H -#define COLLISION_SHAPE_H +#ifndef BT_COLLISION_SHAPE_H +#define BT_COLLISION_SHAPE_H #include "LinearMath/btTransform.h" #include "LinearMath/btVector3.h" @@ -146,5 +146,5 @@ SIMD_FORCE_INLINE int btCollisionShape::calculateSerializeBufferSize() const -#endif //COLLISION_SHAPE_H +#endif //BT_COLLISION_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h index 7f41dd4517b..141034a8e8c 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCompoundShape.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef COMPOUND_SHAPE_H -#define COMPOUND_SHAPE_H +#ifndef BT_COMPOUND_SHAPE_H +#define BT_COMPOUND_SHAPE_H #include "btCollisionShape.h" @@ -209,4 +209,4 @@ SIMD_FORCE_INLINE int btCompoundShape::calculateSerializeBufferSize() const -#endif //COMPOUND_SHAPE_H +#endif //BT_COMPOUND_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.h index 2a370a47c75..2a03241c9d7 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConcaveShape.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef CONCAVE_SHAPE_H -#define CONCAVE_SHAPE_H +#ifndef BT_CONCAVE_SHAPE_H +#define BT_CONCAVE_SHAPE_H #include "btCollisionShape.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types @@ -57,4 +57,4 @@ public: }; -#endif //CONCAVE_SHAPE_H +#endif //BT_CONCAVE_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h index bd7d1443ac2..b69b5c5b0c8 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConeShape.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef CONE_MINKOWSKI_H -#define CONE_MINKOWSKI_H +#ifndef BT_CONE_MINKOWSKI_H +#define BT_CONE_MINKOWSKI_H #include "btConvexInternalShape.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types @@ -99,5 +99,5 @@ class btConeShapeZ : public btConeShape public: btConeShapeZ(btScalar radius,btScalar height); }; -#endif //CONE_MINKOWSKI_H +#endif //BT_CONE_MINKOWSKI_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp index 69bc67cafab..226245979ab 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.cpp @@ -208,4 +208,48 @@ const char* btConvexHullShape::serialize(void* dataBuffer, btSerializer* seriali return "btConvexHullShapeData"; } +void btConvexHullShape::project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max) const +{ +#if 1 + min = FLT_MAX; + max = -FLT_MAX; + btVector3 witnesPtMin; + btVector3 witnesPtMax; + + int numVerts = m_unscaledPoints.size(); + for(int i=0;i<numVerts;i++) + { + btVector3 vtx = m_unscaledPoints[i] * m_localScaling; + btVector3 pt = trans * vtx; + btScalar dp = pt.dot(dir); + if(dp < min) + { + min = dp; + witnesPtMin = pt; + } + if(dp > max) + { + max = dp; + witnesPtMax=pt; + } + } +#else + btVector3 localAxis = dir*trans.getBasis(); + btVector3 vtx1 = trans(localGetSupportingVertex(localAxis)); + btVector3 vtx2 = trans(localGetSupportingVertex(-localAxis)); + + min = vtx1.dot(dir); + max = vtx2.dot(dir); +#endif + + if(min>max) + { + btScalar tmp = min; + min = max; + max = tmp; + } + + +} + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.h index bf960f4df92..95a2af6a3a0 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexHullShape.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef CONVEX_HULL_SHAPE_H -#define CONVEX_HULL_SHAPE_H +#ifndef BT_CONVEX_HULL_SHAPE_H +#define BT_CONVEX_HULL_SHAPE_H #include "btPolyhedralConvexShape.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types @@ -73,6 +73,8 @@ public: virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const; + virtual void project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max) const; + //debugging virtual const char* getName()const {return "Convex";} @@ -116,5 +118,5 @@ SIMD_FORCE_INLINE int btConvexHullShape::calculateSerializeBufferSize() const } -#endif //CONVEX_HULL_SHAPE_H +#endif //BT_CONVEX_HULL_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.h index 12527731804..85cd9ef90c7 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexInternalShape.h @@ -21,6 +21,11 @@ subject to the following restrictions: ///The btConvexInternalShape is an internal base class, shared by most convex shape implementations. +///The btConvexInternalShape uses a default collision margin set to CONVEX_DISTANCE_MARGIN. +///This collision margin used by Gjk and some other algorithms, see also btCollisionMargin.h +///Note that when creating small shapes (derived from btConvexInternalShape), +///you need to make sure to set a smaller collision margin, using the 'setMargin' API +///There is a automatic mechanism 'setSafeMargin' used by btBoxShape and btCylinderShape class btConvexInternalShape : public btConvexShape { @@ -62,6 +67,23 @@ public: m_implicitShapeDimensions = dimensions; } + void setSafeMargin(btScalar minDimension, btScalar defaultMarginMultiplier = 0.1f) + { + btScalar safeMargin = defaultMarginMultiplier*minDimension; + if (safeMargin < getMargin()) + { + setMargin(safeMargin); + } + } + void setSafeMargin(const btVector3& halfExtents, btScalar defaultMarginMultiplier = 0.1f) + { + //see http://code.google.com/p/bullet/issues/detail?id=349 + //this margin check could could be added to other collision shapes too, + //or add some assert/warning somewhere + btScalar minDimension=halfExtents[halfExtents.minAxis()]; + setSafeMargin(minDimension, defaultMarginMultiplier); + } + ///getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const { diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp new file mode 100644 index 00000000000..1e26be531c2 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPolyhedron.cpp @@ -0,0 +1,296 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2011 Advanced Micro Devices, Inc. http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +///This file was written by Erwin Coumans +///Separating axis rest based on work from Pierre Terdiman, see +///And contact clipping based on work from Simon Hobbs + +#include "btConvexPolyhedron.h" +#include "LinearMath/btHashMap.h" + +btConvexPolyhedron::btConvexPolyhedron() +{ + +} +btConvexPolyhedron::~btConvexPolyhedron() +{ + +} + + +inline bool IsAlmostZero(const btVector3& v) +{ + if(fabsf(v.x())>1e-6 || fabsf(v.y())>1e-6 || fabsf(v.z())>1e-6) return false; + return true; +} + +struct btInternalVertexPair +{ + btInternalVertexPair(short int v0,short int v1) + :m_v0(v0), + m_v1(v1) + { + if (m_v1>m_v0) + btSwap(m_v0,m_v1); + } + short int m_v0; + short int m_v1; + int getHash() const + { + return m_v0+(m_v1<<16); + } + bool equals(const btInternalVertexPair& other) const + { + return m_v0==other.m_v0 && m_v1==other.m_v1; + } +}; + +struct btInternalEdge +{ + btInternalEdge() + :m_face0(-1), + m_face1(-1) + { + } + short int m_face0; + short int m_face1; +}; + +// + +#ifdef TEST_INTERNAL_OBJECTS +bool btConvexPolyhedron::testContainment() const +{ + for(int p=0;p<8;p++) + { + btVector3 LocalPt; + if(p==0) LocalPt = m_localCenter + btVector3(m_extents[0], m_extents[1], m_extents[2]); + else if(p==1) LocalPt = m_localCenter + btVector3(m_extents[0], m_extents[1], -m_extents[2]); + else if(p==2) LocalPt = m_localCenter + btVector3(m_extents[0], -m_extents[1], m_extents[2]); + else if(p==3) LocalPt = m_localCenter + btVector3(m_extents[0], -m_extents[1], -m_extents[2]); + else if(p==4) LocalPt = m_localCenter + btVector3(-m_extents[0], m_extents[1], m_extents[2]); + else if(p==5) LocalPt = m_localCenter + btVector3(-m_extents[0], m_extents[1], -m_extents[2]); + else if(p==6) LocalPt = m_localCenter + btVector3(-m_extents[0], -m_extents[1], m_extents[2]); + else if(p==7) LocalPt = m_localCenter + btVector3(-m_extents[0], -m_extents[1], -m_extents[2]); + + for(int i=0;i<m_faces.size();i++) + { + const btVector3 Normal(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]); + const btScalar d = LocalPt.dot(Normal) + m_faces[i].m_plane[3]; + if(d>0.0f) + return false; + } + } + return true; +} +#endif + +void btConvexPolyhedron::initialize() +{ + + btHashMap<btInternalVertexPair,btInternalEdge> edges; + + btScalar TotalArea = 0.0f; + + m_localCenter.setValue(0, 0, 0); + for(int i=0;i<m_faces.size();i++) + { + int numVertices = m_faces[i].m_indices.size(); + int NbTris = numVertices; + for(int j=0;j<NbTris;j++) + { + int k = (j+1)%numVertices; + btInternalVertexPair vp(m_faces[i].m_indices[j],m_faces[i].m_indices[k]); + btInternalEdge* edptr = edges.find(vp); + btVector3 edge = m_vertices[vp.m_v1]-m_vertices[vp.m_v0]; + edge.normalize(); + + bool found = false; + + for (int p=0;p<m_uniqueEdges.size();p++) + { + + if (IsAlmostZero(m_uniqueEdges[p]-edge) || + IsAlmostZero(m_uniqueEdges[p]+edge)) + { + found = true; + break; + } + } + + if (!found) + { + m_uniqueEdges.push_back(edge); + } + + if (edptr) + { + btAssert(edptr->m_face0>=0); + btAssert(edptr->m_face1<0); + edptr->m_face1 = i; + } else + { + btInternalEdge ed; + ed.m_face0 = i; + edges.insert(vp,ed); + } + } + } + +#ifdef USE_CONNECTED_FACES + for(int i=0;i<m_faces.size();i++) + { + int numVertices = m_faces[i].m_indices.size(); + m_faces[i].m_connectedFaces.resize(numVertices); + + for(int j=0;j<numVertices;j++) + { + int k = (j+1)%numVertices; + btInternalVertexPair vp(m_faces[i].m_indices[j],m_faces[i].m_indices[k]); + btInternalEdge* edptr = edges.find(vp); + btAssert(edptr); + btAssert(edptr->m_face0>=0); + btAssert(edptr->m_face1>=0); + + int connectedFace = (edptr->m_face0==i)?edptr->m_face1:edptr->m_face0; + m_faces[i].m_connectedFaces[j] = connectedFace; + } + } +#endif//USE_CONNECTED_FACES + + for(int i=0;i<m_faces.size();i++) + { + int numVertices = m_faces[i].m_indices.size(); + int NbTris = numVertices-2; + + const btVector3& p0 = m_vertices[m_faces[i].m_indices[0]]; + for(int j=1;j<=NbTris;j++) + { + int k = (j+1)%numVertices; + const btVector3& p1 = m_vertices[m_faces[i].m_indices[j]]; + const btVector3& p2 = m_vertices[m_faces[i].m_indices[k]]; + btScalar Area = ((p0 - p1).cross(p0 - p2)).length() * 0.5f; + btVector3 Center = (p0+p1+p2)/3.0f; + m_localCenter += Area * Center; + TotalArea += Area; + } + } + m_localCenter /= TotalArea; + + + + +#ifdef TEST_INTERNAL_OBJECTS + if(1) + { + m_radius = FLT_MAX; + for(int i=0;i<m_faces.size();i++) + { + const btVector3 Normal(m_faces[i].m_plane[0], m_faces[i].m_plane[1], m_faces[i].m_plane[2]); + const btScalar dist = btFabs(m_localCenter.dot(Normal) + m_faces[i].m_plane[3]); + if(dist<m_radius) + m_radius = dist; + } + + + btScalar MinX = FLT_MAX; + btScalar MinY = FLT_MAX; + btScalar MinZ = FLT_MAX; + btScalar MaxX = -FLT_MAX; + btScalar MaxY = -FLT_MAX; + btScalar MaxZ = -FLT_MAX; + for(int i=0; i<m_vertices.size(); i++) + { + const btVector3& pt = m_vertices[i]; + if(pt.x()<MinX) MinX = pt.x(); + if(pt.x()>MaxX) MaxX = pt.x(); + if(pt.y()<MinY) MinY = pt.y(); + if(pt.y()>MaxY) MaxY = pt.y(); + if(pt.z()<MinZ) MinZ = pt.z(); + if(pt.z()>MaxZ) MaxZ = pt.z(); + } + mC.setValue(MaxX+MinX, MaxY+MinY, MaxZ+MinZ); + mE.setValue(MaxX-MinX, MaxY-MinY, MaxZ-MinZ); + + + +// const btScalar r = m_radius / sqrtf(2.0f); + const btScalar r = m_radius / sqrtf(3.0f); + const int LargestExtent = mE.maxAxis(); + const btScalar Step = (mE[LargestExtent]*0.5f - r)/1024.0f; + m_extents[0] = m_extents[1] = m_extents[2] = r; + m_extents[LargestExtent] = mE[LargestExtent]*0.5f; + bool FoundBox = false; + for(int j=0;j<1024;j++) + { + if(testContainment()) + { + FoundBox = true; + break; + } + + m_extents[LargestExtent] -= Step; + } + if(!FoundBox) + { + m_extents[0] = m_extents[1] = m_extents[2] = r; + } + else + { + // Refine the box + const btScalar Step = (m_radius - r)/1024.0f; + const int e0 = (1<<LargestExtent) & 3; + const int e1 = (1<<e0) & 3; + + for(int j=0;j<1024;j++) + { + const btScalar Saved0 = m_extents[e0]; + const btScalar Saved1 = m_extents[e1]; + m_extents[e0] += Step; + m_extents[e1] += Step; + + if(!testContainment()) + { + m_extents[e0] = Saved0; + m_extents[e1] = Saved1; + break; + } + } + } + } +#endif +} + + +void btConvexPolyhedron::project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max) const +{ + min = FLT_MAX; + max = -FLT_MAX; + int numVerts = m_vertices.size(); + for(int i=0;i<numVerts;i++) + { + btVector3 pt = trans * m_vertices[i]; + btScalar dp = pt.dot(dir); + if(dp < min) min = dp; + if(dp > max) max = dp; + } + if(min>max) + { + btScalar tmp = min; + min = max; + max = tmp; + } +}
\ No newline at end of file diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPolyhedron.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPolyhedron.h new file mode 100644 index 00000000000..08db39a33c8 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexPolyhedron.h @@ -0,0 +1,62 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2011 Advanced Micro Devices, Inc. http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +///This file was written by Erwin Coumans + + +#ifndef _BT_POLYHEDRAL_FEATURES_H +#define _BT_POLYHEDRAL_FEATURES_H + +#include "LinearMath/btTransform.h" +#include "LinearMath/btAlignedObjectArray.h" + +#define TEST_INTERNAL_OBJECTS 1 + + +struct btFace +{ + btAlignedObjectArray<int> m_indices; +// btAlignedObjectArray<int> m_connectedFaces; + btScalar m_plane[4]; +}; + + +class btConvexPolyhedron +{ + public: + btConvexPolyhedron(); + virtual ~btConvexPolyhedron(); + + btAlignedObjectArray<btVector3> m_vertices; + btAlignedObjectArray<btFace> m_faces; + btAlignedObjectArray<btVector3> m_uniqueEdges; + + btVector3 m_localCenter; + btVector3 m_extents; + btScalar m_radius; + btVector3 mC; + btVector3 mE; + + void initialize(); + bool testContainment() const; + + void project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max) const; +}; + + +#endif //_BT_POLYHEDRAL_FEATURES_H + + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp index f5f3aa58aa4..8c67d8ebef1 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.cpp @@ -43,6 +43,23 @@ btConvexShape::~btConvexShape() } +void btConvexShape::project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max) const +{ + btVector3 localAxis = dir*trans.getBasis(); + btVector3 vtx1 = trans(localGetSupportingVertex(localAxis)); + btVector3 vtx2 = trans(localGetSupportingVertex(-localAxis)); + + min = vtx1.dot(dir); + max = vtx2.dot(dir); + + if(min>max) + { + btScalar tmp = min; + min = max; + max = tmp; + } +} + static btVector3 convexHullSupport (const btVector3& localDirOrg, const btVector3* points, int numPoints, const btVector3& localScaling) { @@ -227,7 +244,7 @@ btVector3 btConvexShape::localGetSupportVertexWithoutMarginNonVirtual (const btV pos[capsuleUpAxis] = halfHeight; //vtx = pos +vec*(radius); - vtx = pos +vec*capsuleShape->getLocalScalingNV()*(radius) - vec * capsuleShape->getMarginNV(); + vtx = pos +vec*(radius) - vec * capsuleShape->getMarginNV(); newDot = vec.dot(vtx); @@ -242,7 +259,7 @@ btVector3 btConvexShape::localGetSupportVertexWithoutMarginNonVirtual (const btV pos[capsuleUpAxis] = -halfHeight; //vtx = pos +vec*(radius); - vtx = pos +vec*capsuleShape->getLocalScalingNV()*(radius) - vec * capsuleShape->getMarginNV(); + vtx = pos +vec*(radius) - vec * capsuleShape->getMarginNV(); newDot = vec.dot(vtx); if (newDot > maxDot) { diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h index 9c158259c1c..290cd9fd13c 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexShape.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef CONVEX_SHAPE_INTERFACE1 -#define CONVEX_SHAPE_INTERFACE1 +#ifndef BT_CONVEX_SHAPE_INTERFACE1 +#define BT_CONVEX_SHAPE_INTERFACE1 #include "btCollisionShape.h" @@ -52,6 +52,8 @@ public: btScalar getMarginNonVirtual () const; void getAabbNonVirtual (const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const; + virtual void project(const btTransform& trans, const btVector3& dir, btScalar& min, btScalar& max) const; + //notice that the vectors should be unit length virtual void batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const= 0; @@ -79,4 +81,4 @@ public: -#endif //CONVEX_SHAPE_INTERFACE1 +#endif //BT_CONVEX_SHAPE_INTERFACE1 diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h index f5167e74b80..af5d00388e8 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h @@ -12,8 +12,8 @@ subject to the following restrictions: 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ -#ifndef CONVEX_TRIANGLEMESH_SHAPE_H -#define CONVEX_TRIANGLEMESH_SHAPE_H +#ifndef BT_CONVEX_TRIANGLEMESH_SHAPE_H +#define BT_CONVEX_TRIANGLEMESH_SHAPE_H #include "btPolyhedralConvexShape.h" @@ -69,7 +69,7 @@ public: -#endif //CONVEX_TRIANGLEMESH_SHAPE_H +#endif //BT_CONVEX_TRIANGLEMESH_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.cpp index c2e534b0b46..6cfe43be4da 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.cpp @@ -19,6 +19,8 @@ btCylinderShape::btCylinderShape (const btVector3& halfExtents) :btConvexInternalShape(), m_upAxis(1) { + setSafeMargin(halfExtents); + btVector3 margin(getMargin(),getMargin(),getMargin()); m_implicitShapeDimensions = (halfExtents * m_localScaling) - margin; m_shapeType = CYLINDER_SHAPE_PROXYTYPE; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.h index f7899265d31..125bfc78a77 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btCylinderShape.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef CYLINDER_MINKOWSKI_H -#define CYLINDER_MINKOWSKI_H +#ifndef BT_CYLINDER_MINKOWSKI_H +#define BT_CYLINDER_MINKOWSKI_H #include "btBoxShape.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types @@ -196,5 +196,5 @@ SIMD_FORCE_INLINE const char* btCylinderShape::serialize(void* dataBuffer, btSer -#endif //CYLINDER_MINKOWSKI_H +#endif //BT_CYLINDER_MINKOWSKI_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.h index 9f6b4435c29..87b7b66d1e1 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btEmptyShape.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef EMPTY_SHAPE_H -#define EMPTY_SHAPE_H +#ifndef BT_EMPTY_SHAPE_H +#define BT_EMPTY_SHAPE_H #include "btConcaveShape.h" @@ -67,4 +67,4 @@ protected: -#endif //EMPTY_SHAPE_H +#endif //BT_EMPTY_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp index 3a1e6f4a2b9..95631c30190 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.cpp @@ -21,7 +21,7 @@ subject to the following restrictions: btHeightfieldTerrainShape::btHeightfieldTerrainShape ( -int heightStickWidth, int heightStickLength, void* heightfieldData, +int heightStickWidth, int heightStickLength, const void* heightfieldData, btScalar heightScale, btScalar minHeight, btScalar maxHeight,int upAxis, PHY_ScalarType hdt, bool flipQuadEdges ) @@ -33,7 +33,7 @@ PHY_ScalarType hdt, bool flipQuadEdges -btHeightfieldTerrainShape::btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength,void* heightfieldData,btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges) +btHeightfieldTerrainShape::btHeightfieldTerrainShape(int heightStickWidth, int heightStickLength,const void* heightfieldData,btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges) { // legacy constructor: support only float or unsigned char, // and min height is zero @@ -53,7 +53,7 @@ btHeightfieldTerrainShape::btHeightfieldTerrainShape(int heightStickWidth, int h void btHeightfieldTerrainShape::initialize ( -int heightStickWidth, int heightStickLength, void* heightfieldData, +int heightStickWidth, int heightStickLength, const void* heightfieldData, btScalar heightScale, btScalar minHeight, btScalar maxHeight, int upAxis, PHY_ScalarType hdt, bool flipQuadEdges ) diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h index 4f5d1e35bf3..78e231e08e6 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btHeightfieldTerrainShape.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef HEIGHTFIELD_TERRAIN_SHAPE_H -#define HEIGHTFIELD_TERRAIN_SHAPE_H +#ifndef BT_HEIGHTFIELD_TERRAIN_SHAPE_H +#define BT_HEIGHTFIELD_TERRAIN_SHAPE_H #include "btConcaveShape.h" @@ -85,10 +85,10 @@ protected: btScalar m_heightScale; union { - unsigned char* m_heightfieldDataUnsignedChar; - short* m_heightfieldDataShort; - btScalar* m_heightfieldDataFloat; - void* m_heightfieldDataUnknown; + const unsigned char* m_heightfieldDataUnsignedChar; + const short* m_heightfieldDataShort; + const btScalar* m_heightfieldDataFloat; + const void* m_heightfieldDataUnknown; }; PHY_ScalarType m_heightDataType; @@ -111,7 +111,7 @@ protected: backwards-compatible without a lot of copy/paste. */ void initialize(int heightStickWidth, int heightStickLength, - void* heightfieldData, btScalar heightScale, + const void* heightfieldData, btScalar heightScale, btScalar minHeight, btScalar maxHeight, int upAxis, PHY_ScalarType heightDataType, bool flipQuadEdges); @@ -123,7 +123,7 @@ public: heightScale is needed for any integer-based heightfield data types. */ btHeightfieldTerrainShape(int heightStickWidth,int heightStickLength, - void* heightfieldData, btScalar heightScale, + const void* heightfieldData, btScalar heightScale, btScalar minHeight, btScalar maxHeight, int upAxis, PHY_ScalarType heightDataType, bool flipQuadEdges); @@ -135,7 +135,7 @@ public: compatibility reasons, heightScale is calculated as maxHeight / 65535 (and is only used when useFloatData = false). */ - btHeightfieldTerrainShape(int heightStickWidth,int heightStickLength,void* heightfieldData, btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges); + btHeightfieldTerrainShape(int heightStickWidth,int heightStickLength,const void* heightfieldData, btScalar maxHeight,int upAxis,bool useFloatData,bool flipQuadEdges); virtual ~btHeightfieldTerrainShape(); @@ -158,4 +158,4 @@ public: }; -#endif //HEIGHTFIELD_TERRAIN_SHAPE_H +#endif //BT_HEIGHTFIELD_TERRAIN_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMaterial.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btMaterial.h index 030e167da50..866f9b4da4d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMaterial.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMaterial.h @@ -15,8 +15,8 @@ subject to the following restrictions: /// This file was created by Alex Silverman -#ifndef MATERIAL_H -#define MATERIAL_H +#ifndef BT_MATERIAL_H +#define BT_MATERIAL_H // Material class to be used by btMultimaterialTriangleMeshShape to store triangle properties class btMaterial @@ -31,5 +31,5 @@ public: btMaterial(btScalar fric, btScalar rest) { m_friction = fric; m_restitution = rest; } }; -#endif // MATERIAL_H +#endif // BT_MATERIAL_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h index d6fd040213b..6c844e8c0af 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMinkowskiSumShape.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef MINKOWSKI_SUM_SHAPE_H -#define MINKOWSKI_SUM_SHAPE_H +#ifndef BT_MINKOWSKI_SUM_SHAPE_H +#define BT_MINKOWSKI_SUM_SHAPE_H #include "btConvexInternalShape.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types @@ -57,4 +57,4 @@ public: } }; -#endif //MINKOWSKI_SUM_SHAPE_H +#endif //BT_MINKOWSKI_SUM_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h index 3db7e320889..06c5d16d941 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultiSphereShape.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef MULTI_SPHERE_MINKOWSKI_H -#define MULTI_SPHERE_MINKOWSKI_H +#ifndef BT_MULTI_SPHERE_MINKOWSKI_H +#define BT_MULTI_SPHERE_MINKOWSKI_H #include "btConvexInternalShape.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types @@ -96,4 +96,4 @@ SIMD_FORCE_INLINE int btMultiSphereShape::calculateSerializeBufferSize() const -#endif //MULTI_SPHERE_MINKOWSKI_H +#endif //BT_MULTI_SPHERE_MINKOWSKI_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h index 96b2ad95b6d..2b92ab7d1b7 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btMultimaterialTriangleMeshShape.h @@ -15,8 +15,8 @@ subject to the following restrictions: /// This file was created by Alex Silverman -#ifndef BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H -#define BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H +#ifndef BT_BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H +#define BT_BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H #include "btBvhTriangleMeshShape.h" #include "btMaterial.h" @@ -117,4 +117,4 @@ public: } ; -#endif //BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H +#endif //BT_BVH_TRIANGLE_MATERIAL_MESH_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp index 981b8a2652c..6f36775f7c9 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.cpp @@ -43,7 +43,7 @@ void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantized NodeTriangleCallback& operator=(NodeTriangleCallback& other) { - m_triangleNodes = other.m_triangleNodes; + m_triangleNodes.copyFromArray(other.m_triangleNodes); return *this; } @@ -84,7 +84,7 @@ void btOptimizedBvh::build(btStridingMeshInterface* triangles, bool useQuantized QuantizedNodeTriangleCallback& operator=(QuantizedNodeTriangleCallback& other) { - m_triangleNodes = other.m_triangleNodes; + m_triangleNodes.copyFromArray(other.m_triangleNodes); m_optimizedTree = other.m_optimizedTree; return *this; } diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.h index 749fe6005dd..715961f5528 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btOptimizedBvh.h @@ -15,8 +15,8 @@ subject to the following restrictions: ///Contains contributions from Disney Studio's -#ifndef OPTIMIZED_BVH_H -#define OPTIMIZED_BVH_H +#ifndef BT_OPTIMIZED_BVH_H +#define BT_OPTIMIZED_BVH_H #include "BulletCollision/BroadphaseCollision/btQuantizedBvh.h" @@ -60,6 +60,6 @@ public: }; -#endif //OPTIMIZED_BVH_H +#endif //BT_OPTIMIZED_BVH_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp index b1ecb3e432c..82def79cf55 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.cpp @@ -14,10 +14,289 @@ subject to the following restrictions: */ #include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h" +#include "btConvexPolyhedron.h" +#include "LinearMath/btConvexHullComputer.h" +#include <new> +#include "LinearMath/btGeometryUtil.h" +#include "LinearMath/btGrahamScan2dConvexHull.h" -btPolyhedralConvexShape::btPolyhedralConvexShape() :btConvexInternalShape() + +btPolyhedralConvexShape::btPolyhedralConvexShape() :btConvexInternalShape(), +m_polyhedron(0) +{ + +} + +btPolyhedralConvexShape::~btPolyhedralConvexShape() +{ + if (m_polyhedron) + { + btAlignedFree(m_polyhedron); + } +} + + +bool btPolyhedralConvexShape::initializePolyhedralFeatures() { + if (m_polyhedron) + btAlignedFree(m_polyhedron); + + void* mem = btAlignedAlloc(sizeof(btConvexPolyhedron),16); + m_polyhedron = new (mem) btConvexPolyhedron; + + btAlignedObjectArray<btVector3> orgVertices; + + for (int i=0;i<getNumVertices();i++) + { + btVector3& newVertex = orgVertices.expand(); + getVertex(i,newVertex); + } + +#if 0 + btAlignedObjectArray<btVector3> planeEquations; + btGeometryUtil::getPlaneEquationsFromVertices(orgVertices,planeEquations); + + btAlignedObjectArray<btVector3> shiftedPlaneEquations; + for (int p=0;p<planeEquations.size();p++) + { + btVector3 plane = planeEquations[p]; + plane[3] -= getMargin(); + shiftedPlaneEquations.push_back(plane); + } + + btAlignedObjectArray<btVector3> tmpVertices; + + btGeometryUtil::getVerticesFromPlaneEquations(shiftedPlaneEquations,tmpVertices); + btConvexHullComputer conv; + conv.compute(&tmpVertices[0].getX(), sizeof(btVector3),tmpVertices.size(),0.f,0.f); + +#else + btConvexHullComputer conv; + conv.compute(&orgVertices[0].getX(), sizeof(btVector3),orgVertices.size(),0.f,0.f); + +#endif + + + + btAlignedObjectArray<btVector3> faceNormals; + int numFaces = conv.faces.size(); + faceNormals.resize(numFaces); + btConvexHullComputer* convexUtil = &conv; + + + btAlignedObjectArray<btFace> tmpFaces; + tmpFaces.resize(numFaces); + + int numVertices = convexUtil->vertices.size(); + m_polyhedron->m_vertices.resize(numVertices); + for (int p=0;p<numVertices;p++) + { + m_polyhedron->m_vertices[p] = convexUtil->vertices[p]; + } + + + for (int i=0;i<numFaces;i++) + { + int face = convexUtil->faces[i]; + //printf("face=%d\n",face); + const btConvexHullComputer::Edge* firstEdge = &convexUtil->edges[face]; + const btConvexHullComputer::Edge* edge = firstEdge; + + btVector3 edges[3]; + int numEdges = 0; + //compute face normals + + btScalar maxCross2 = 0.f; + int chosenEdge = -1; + + do + { + + int src = edge->getSourceVertex(); + tmpFaces[i].m_indices.push_back(src); + int targ = edge->getTargetVertex(); + btVector3 wa = convexUtil->vertices[src]; + + btVector3 wb = convexUtil->vertices[targ]; + btVector3 newEdge = wb-wa; + newEdge.normalize(); + if (numEdges<2) + edges[numEdges++] = newEdge; + + edge = edge->getNextEdgeOfFace(); + } while (edge!=firstEdge); + + btScalar planeEq = 1e30f; + + + if (numEdges==2) + { + faceNormals[i] = edges[0].cross(edges[1]); + faceNormals[i].normalize(); + tmpFaces[i].m_plane[0] = faceNormals[i].getX(); + tmpFaces[i].m_plane[1] = faceNormals[i].getY(); + tmpFaces[i].m_plane[2] = faceNormals[i].getZ(); + tmpFaces[i].m_plane[3] = planeEq; + + } + else + { + btAssert(0);//degenerate? + faceNormals[i].setZero(); + } + + for (int v=0;v<tmpFaces[i].m_indices.size();v++) + { + btScalar eq = m_polyhedron->m_vertices[tmpFaces[i].m_indices[v]].dot(faceNormals[i]); + if (planeEq>eq) + { + planeEq=eq; + } + } + tmpFaces[i].m_plane[3] = -planeEq; + } + + //merge coplanar faces and copy them to m_polyhedron + + btScalar faceWeldThreshold= 0.999f; + btAlignedObjectArray<int> todoFaces; + for (int i=0;i<tmpFaces.size();i++) + todoFaces.push_back(i); + + while (todoFaces.size()) + { + btAlignedObjectArray<int> coplanarFaceGroup; + int refFace = todoFaces[todoFaces.size()-1]; + + coplanarFaceGroup.push_back(refFace); + btFace& faceA = tmpFaces[refFace]; + todoFaces.pop_back(); + + btVector3 faceNormalA(faceA.m_plane[0],faceA.m_plane[1],faceA.m_plane[2]); + for (int j=todoFaces.size()-1;j>=0;j--) + { + int i = todoFaces[j]; + btFace& faceB = tmpFaces[i]; + btVector3 faceNormalB(faceB.m_plane[0],faceB.m_plane[1],faceB.m_plane[2]); + if (faceNormalA.dot(faceNormalB)>faceWeldThreshold) + { + coplanarFaceGroup.push_back(i); + todoFaces.remove(i); + } + } + + + bool did_merge = false; + if (coplanarFaceGroup.size()>1) + { + //do the merge: use Graham Scan 2d convex hull + + btAlignedObjectArray<GrahamVector2> orgpoints; + + for (int i=0;i<coplanarFaceGroup.size();i++) + { +// m_polyhedron->m_faces.push_back(tmpFaces[coplanarFaceGroup[i]]); + + btFace& face = tmpFaces[coplanarFaceGroup[i]]; + btVector3 faceNormal(face.m_plane[0],face.m_plane[1],face.m_plane[2]); + btVector3 xyPlaneNormal(0,0,1); + + btQuaternion rotationArc = shortestArcQuat(faceNormal,xyPlaneNormal); + + for (int f=0;f<face.m_indices.size();f++) + { + int orgIndex = face.m_indices[f]; + btVector3 pt = m_polyhedron->m_vertices[orgIndex]; + btVector3 rotatedPt = quatRotate(rotationArc,pt); + rotatedPt.setZ(0); + bool found = false; + + for (int i=0;i<orgpoints.size();i++) + { + //if ((orgpoints[i].m_orgIndex == orgIndex) || ((rotatedPt-orgpoints[i]).length2()<0.0001)) + if (orgpoints[i].m_orgIndex == orgIndex) + { + found=true; + break; + } + } + if (!found) + orgpoints.push_back(GrahamVector2(rotatedPt,orgIndex)); + } + } + + btFace combinedFace; + for (int i=0;i<4;i++) + combinedFace.m_plane[i] = tmpFaces[coplanarFaceGroup[0]].m_plane[i]; + + btAlignedObjectArray<GrahamVector2> hull; + GrahamScanConvexHull2D(orgpoints,hull); + + for (int i=0;i<hull.size();i++) + { + combinedFace.m_indices.push_back(hull[i].m_orgIndex); + for(int k = 0; k < orgpoints.size(); k++) { + if(orgpoints[k].m_orgIndex == hull[i].m_orgIndex) { + orgpoints[k].m_orgIndex = -1; // invalidate... + break; + } + } + } + // are there rejected vertices? + bool reject_merge = false; + for(int i = 0; i < orgpoints.size(); i++) { + if(orgpoints[i].m_orgIndex == -1) + continue; // this is in the hull... + // this vertex is rejected -- is anybody else using this vertex? + for(int j = 0; j < tmpFaces.size(); j++) { + btFace& face = tmpFaces[j]; + // is this a face of the current coplanar group? + bool is_in_current_group = false; + for(int k = 0; k < coplanarFaceGroup.size(); k++) { + if(coplanarFaceGroup[k] == j) { + is_in_current_group = true; + break; + } + } + if(is_in_current_group) // ignore this face... + continue; + // does this face use this rejected vertex? + for(int v = 0; v < face.m_indices.size(); v++) { + if(face.m_indices[v] == orgpoints[i].m_orgIndex) { + // this rejected vertex is used in another face -- reject merge + reject_merge = true; + break; + } + } + if(reject_merge) + break; + } + if(reject_merge) + break; + } + if(!reject_merge) { + // do this merge! + did_merge = true; + m_polyhedron->m_faces.push_back(combinedFace); + } + } + if(!did_merge) + { + for (int i=0;i<coplanarFaceGroup.size();i++) + { + m_polyhedron->m_faces.push_back(tmpFaces[coplanarFaceGroup[i]]); + } + + } + + + + } + + m_polyhedron->initialize(); + + return true; } @@ -183,11 +462,14 @@ void btPolyhedralConvexAabbCachingShape::recalcLocalAabb() btVector3 vec(btScalar(0.),btScalar(0.),btScalar(0.)); vec[i] = btScalar(1.); btVector3 tmp = localGetSupportingVertex(vec); - m_localAabbMax[i] = tmp[i]+m_collisionMargin; + m_localAabbMax[i] = tmp[i]; vec[i] = btScalar(-1.); tmp = localGetSupportingVertex(vec); - m_localAabbMin[i] = tmp[i]-m_collisionMargin; + m_localAabbMin[i] = tmp[i]; } #endif } + + + diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h index 2c691b95652..ee2e1e28277 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btPolyhedralConvexShape.h @@ -13,23 +13,37 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef BU_SHAPE -#define BU_SHAPE +#ifndef BT_POLYHEDRAL_CONVEX_SHAPE_H +#define BT_POLYHEDRAL_CONVEX_SHAPE_H #include "LinearMath/btMatrix3x3.h" #include "btConvexInternalShape.h" +class btConvexPolyhedron; ///The btPolyhedralConvexShape is an internal interface class for polyhedral convex shapes. class btPolyhedralConvexShape : public btConvexInternalShape { + protected: + btConvexPolyhedron* m_polyhedron; + public: btPolyhedralConvexShape(); + virtual ~btPolyhedralConvexShape(); + + ///optional method mainly used to generate multiple contact points by clipping polyhedral features (faces/edges) + virtual bool initializePolyhedralFeatures(); + + const btConvexPolyhedron* getConvexPolyhedron() const + { + return m_polyhedron; + } + //brute force implementations virtual btVector3 localGetSupportingVertexWithoutMargin(const btVector3& vec)const; @@ -95,4 +109,4 @@ public: }; -#endif //BU_SHAPE +#endif //BT_POLYHEDRAL_CONVEX_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h index 4ab28555ba8..ff86ef319e9 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef SCALED_BVH_TRIANGLE_MESH_SHAPE_H -#define SCALED_BVH_TRIANGLE_MESH_SHAPE_H +#ifndef BT_SCALED_BVH_TRIANGLE_MESH_SHAPE_H +#define BT_SCALED_BVH_TRIANGLE_MESH_SHAPE_H #include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h" @@ -90,4 +90,4 @@ SIMD_FORCE_INLINE const char* btScaledBvhTriangleMeshShape::serialize(void* data } -#endif //BVH_TRIANGLE_MESH_SHAPE_H +#endif //BT_SCALED_BVH_TRIANGLE_MESH_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.h index 07b3500f994..642a2887435 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btShapeHull.h @@ -15,8 +15,8 @@ subject to the following restrictions: ///btShapeHull implemented by John McCutchan. -#ifndef _SHAPE_HULL_H -#define _SHAPE_HULL_H +#ifndef BT_SHAPE_HULL_H +#define BT_SHAPE_HULL_H #include "LinearMath/btAlignedObjectArray.h" #include "BulletCollision/CollisionShapes/btConvexShape.h" @@ -56,4 +56,4 @@ public: } }; -#endif //_SHAPE_HULL_H +#endif //BT_SHAPE_HULL_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.h index f98372442ba..b192efeeb8d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btSphereShape.h @@ -12,8 +12,8 @@ subject to the following restrictions: 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 3. This notice may not be removed or altered from any source distribution. */ -#ifndef SPHERE_MINKOWSKI_H -#define SPHERE_MINKOWSKI_H +#ifndef BT_SPHERE_MINKOWSKI_H +#define BT_SPHERE_MINKOWSKI_H #include "btConvexInternalShape.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" // for the types @@ -70,4 +70,4 @@ public: }; -#endif //SPHERE_MINKOWSKI_H +#endif //BT_SPHERE_MINKOWSKI_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h index beb53ef33a1..b13825e610d 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btStaticPlaneShape.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef STATIC_PLANE_SHAPE_H -#define STATIC_PLANE_SHAPE_H +#ifndef BT_STATIC_PLANE_SHAPE_H +#define BT_STATIC_PLANE_SHAPE_H #include "btConcaveShape.h" @@ -97,7 +97,7 @@ SIMD_FORCE_INLINE const char* btStaticPlaneShape::serialize(void* dataBuffer, bt } -#endif //STATIC_PLANE_SHAPE_H +#endif //BT_STATIC_PLANE_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp b/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp index bc2f9f21448..dd22fc5635a 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.cpp @@ -253,9 +253,11 @@ const char* btStridingMeshInterface::serialize(void* dataBuffer, btSerializer* s memPtr->m_indices16 = 0; memPtr->m_indices32 = 0; memPtr->m_3indices16 = 0; + memPtr->m_3indices8 = 0; memPtr->m_vertices3f = 0; memPtr->m_vertices3d = 0; + switch (gfxindextype) { case PHY_INTEGER: diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.h index 9e3e2ab0f59..f2b27ade8e8 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btStridingMeshInterface.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef STRIDING_MESHINTERFACE_H -#define STRIDING_MESHINTERFACE_H +#ifndef BT_STRIDING_MESHINTERFACE_H +#define BT_STRIDING_MESHINTERFACE_H #include "LinearMath/btVector3.h" #include "btTriangleCallback.h" @@ -159,4 +159,4 @@ SIMD_FORCE_INLINE int btStridingMeshInterface::calculateSerializeBufferSize() co -#endif //STRIDING_MESHINTERFACE_H +#endif //BT_STRIDING_MESHINTERFACE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.h index 72e9f232876..6b7128efc8e 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTetrahedronShape.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef BU_SIMPLEX_1TO4_SHAPE -#define BU_SIMPLEX_1TO4_SHAPE +#ifndef BT_SIMPLEX_1TO4_SHAPE +#define BT_SIMPLEX_1TO4_SHAPE #include "btPolyhedralConvexShape.h" @@ -71,4 +71,4 @@ public: }; -#endif //BU_SIMPLEX_1TO4_SHAPE +#endif //BT_SIMPLEX_1TO4_SHAPE diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.h index 0499702b05b..461c57f8773 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleCallback.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef TRIANGLE_CALLBACK_H -#define TRIANGLE_CALLBACK_H +#ifndef BT_TRIANGLE_CALLBACK_H +#define BT_TRIANGLE_CALLBACK_H #include "LinearMath/btVector3.h" @@ -39,4 +39,4 @@ public: -#endif //TRIANGLE_CALLBACK_H +#endif //BT_TRIANGLE_CALLBACK_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h index c64ea6e7043..9e1544e87a4 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h @@ -29,9 +29,11 @@ ATTRIBUTE_ALIGNED16( struct) btIndexedMesh int m_numTriangles; const unsigned char * m_triangleIndexBase; + // Size in byte of the indices for one triangle (3*sizeof(index_type) if the indices are tightly packed) int m_triangleIndexStride; int m_numVertices; const unsigned char * m_vertexBase; + // Size of a vertex, in bytes int m_vertexStride; // The index type is set when adding an indexed mesh to the diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleInfoMap.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleInfoMap.h index 282a7702e80..1cea7045f20 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleInfoMap.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleInfoMap.h @@ -61,6 +61,7 @@ struct btTriangleInfoMap : public btInternalTriangleInfoMap btScalar m_planarEpsilon; ///used to determine if a triangle edge is planar with zero angle btScalar m_equalVertexThreshold; ///used to compute connectivity: if the distance between two vertices is smaller than m_equalVertexThreshold, they are considered to be 'shared' btScalar m_edgeDistanceThreshold; ///used to determine edge contacts: if the closest distance between a contact point and an edge is smaller than this distance threshold it is considered to "hit the edge" + btScalar m_maxEdgeAngleThreshold; //ignore edges that connect triangles at an angle larger than this m_maxEdgeAngleThreshold btScalar m_zeroAreaThreshold; ///used to determine if a triangle is degenerate (length squared of cross product of 2 triangle edges < threshold) @@ -71,6 +72,7 @@ struct btTriangleInfoMap : public btInternalTriangleInfoMap m_equalVertexThreshold = btScalar(0.0001)*btScalar(0.0001); m_edgeDistanceThreshold = btScalar(0.1); m_zeroAreaThreshold = btScalar(0.0001)*btScalar(0.0001); + m_maxEdgeAngleThreshold = SIMD_2_PI; } virtual ~btTriangleInfoMap() {} @@ -83,6 +85,7 @@ struct btTriangleInfoMap : public btInternalTriangleInfoMap }; +///those fields have to be float and not btScalar for the serialization to work properly struct btTriangleInfoData { int m_flags; diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h index d2624fe18e1..f623157fac1 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMesh.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef TRIANGLE_MESH_H -#define TRIANGLE_MESH_H +#ifndef BT_TRIANGLE_MESH_H +#define BT_TRIANGLE_MESH_H #include "btTriangleIndexVertexArray.h" #include "LinearMath/btVector3.h" @@ -65,5 +65,5 @@ class btTriangleMesh : public btTriangleIndexVertexArray }; -#endif //TRIANGLE_MESH_H +#endif //BT_TRIANGLE_MESH_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h index 2216698d275..c8caf8fe696 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleMeshShape.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef TRIANGLE_MESH_SHAPE_H -#define TRIANGLE_MESH_SHAPE_H +#ifndef BT_TRIANGLE_MESH_SHAPE_H +#define BT_TRIANGLE_MESH_SHAPE_H #include "btConcaveShape.h" #include "btStridingMeshInterface.h" @@ -86,4 +86,4 @@ public: -#endif //TRIANGLE_MESH_SHAPE_H +#endif //BT_TRIANGLE_MESH_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h index 847147cf6b4..71b0557384e 100644 --- a/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h +++ b/extern/bullet2/src/BulletCollision/CollisionShapes/btTriangleShape.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef OBB_TRIANGLE_MINKOWSKI_H -#define OBB_TRIANGLE_MINKOWSKI_H +#ifndef BT_OBB_TRIANGLE_MINKOWSKI_H +#define BT_OBB_TRIANGLE_MINKOWSKI_H #include "btConvexShape.h" #include "btBoxShape.h" @@ -178,5 +178,5 @@ public: }; -#endif //OBB_TRIANGLE_MINKOWSKI_H +#endif //BT_OBB_TRIANGLE_MINKOWSKI_H diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessing.cpp b/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessing.cpp index c3b697bdd1e..eed31d839f8 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessing.cpp +++ b/extern/bullet2/src/BulletCollision/Gimpact/btContactProcessing.cpp @@ -58,7 +58,7 @@ class CONTACT_KEY_TOKEN_COMP { public: - bool operator() ( const CONTACT_KEY_TOKEN& a, const CONTACT_KEY_TOKEN& b ) + bool operator() ( const CONTACT_KEY_TOKEN& a, const CONTACT_KEY_TOKEN& b ) const { return ( a < b ); } diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h index ae5d55ab7d9..6b6e07c983d 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h @@ -21,8 +21,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef BVH_CONCAVE_COLLISION_ALGORITHM_H -#define BVH_CONCAVE_COLLISION_ALGORITHM_H +#ifndef BT_GIMPACT_BVH_CONCAVE_COLLISION_ALGORITHM_H +#define BT_GIMPACT_BVH_CONCAVE_COLLISION_ALGORITHM_H #include "BulletCollision/CollisionDispatch/btActivatingCollisionAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btDispatcher.h" @@ -303,4 +303,4 @@ public: -#endif //BVH_CONCAVE_COLLISION_ALGORITHM_H +#endif //BT_GIMPACT_BVH_CONCAVE_COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btQuantization.h b/extern/bullet2/src/BulletCollision/Gimpact/btQuantization.h index e0487c22766..bd2633cfc59 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btQuantization.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btQuantization.h @@ -1,5 +1,5 @@ -#ifndef BT_QUANTIZATION_H_INCLUDED -#define BT_QUANTIZATION_H_INCLUDED +#ifndef BT_GIMPACT_QUANTIZATION_H_INCLUDED +#define BT_GIMPACT_QUANTIZATION_H_INCLUDED /*! \file btQuantization.h *\author Francisco Leon Najera @@ -85,4 +85,4 @@ SIMD_FORCE_INLINE btVector3 bt_unquantize( -#endif // GIM_VECTOR_H_INCLUDED +#endif // BT_GIMPACT_QUANTIZATION_H_INCLUDED diff --git a/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.h b/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.h index fcc981dfb69..973c2ed1277 100644 --- a/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.h +++ b/extern/bullet2/src/BulletCollision/Gimpact/btTriangleShapeEx.h @@ -22,8 +22,8 @@ subject to the following restrictions: */ -#ifndef TRIANGLE_SHAPE_EX_H -#define TRIANGLE_SHAPE_EX_H +#ifndef GIMPACT_TRIANGLE_SHAPE_EX_H +#define GIMPACT_TRIANGLE_SHAPE_EX_H #include "BulletCollision/CollisionShapes/btCollisionShape.h" #include "BulletCollision/CollisionShapes/btTriangleShape.h" @@ -177,4 +177,4 @@ public: }; -#endif //TRIANGLE_MESH_SHAPE_H +#endif //GIMPACT_TRIANGLE_MESH_SHAPE_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp index 9ee83e7d561..91fcea57a3c 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.cpp @@ -22,20 +22,72 @@ subject to the following restrictions: #include "btGjkPairDetector.h" #include "btPointCollector.h" +#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h" btContinuousConvexCollision::btContinuousConvexCollision ( const btConvexShape* convexA,const btConvexShape* convexB,btSimplexSolverInterface* simplexSolver, btConvexPenetrationDepthSolver* penetrationDepthSolver) :m_simplexSolver(simplexSolver), m_penetrationDepthSolver(penetrationDepthSolver), -m_convexA(convexA),m_convexB(convexB) +m_convexA(convexA),m_convexB1(convexB),m_planeShape(0) { } + +btContinuousConvexCollision::btContinuousConvexCollision( const btConvexShape* convexA,const btStaticPlaneShape* plane) +:m_simplexSolver(0), +m_penetrationDepthSolver(0), +m_convexA(convexA),m_convexB1(0),m_planeShape(plane) +{ +} + + /// This maximum should not be necessary. It allows for untested/degenerate cases in production code. /// You don't want your game ever to lock-up. #define MAX_ITERATIONS 64 +void btContinuousConvexCollision::computeClosestPoints( const btTransform& transA, const btTransform& transB,btPointCollector& pointCollector) +{ + if (m_convexB1) + { + m_simplexSolver->reset(); + btGjkPairDetector gjk(m_convexA,m_convexB1,m_convexA->getShapeType(),m_convexB1->getShapeType(),m_convexA->getMargin(),m_convexB1->getMargin(),m_simplexSolver,m_penetrationDepthSolver); + btGjkPairDetector::ClosestPointInput input; + input.m_transformA = transA; + input.m_transformB = transB; + gjk.getClosestPoints(input,pointCollector,0); + } else + { + //convex versus plane + const btConvexShape* convexShape = m_convexA; + const btStaticPlaneShape* planeShape = m_planeShape; + + bool hasCollision = false; + const btVector3& planeNormal = planeShape->getPlaneNormal(); + const btScalar& planeConstant = planeShape->getPlaneConstant(); + + btTransform convexWorldTransform = transA; + btTransform convexInPlaneTrans; + convexInPlaneTrans= transB.inverse() * convexWorldTransform; + btTransform planeInConvex; + planeInConvex= convexWorldTransform.inverse() * transB; + + btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal); + + btVector3 vtxInPlane = convexInPlaneTrans(vtx); + btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant); + + btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal; + btVector3 vtxInPlaneWorld = transB * vtxInPlaneProjected; + btVector3 normalOnSurfaceB = transB.getBasis() * planeNormal; + + pointCollector.addContactPoint( + normalOnSurfaceB, + vtxInPlaneWorld, + distance); + } +} + bool btContinuousConvexCollision::calcTimeOfImpact( const btTransform& fromA, const btTransform& toA, @@ -44,7 +96,6 @@ bool btContinuousConvexCollision::calcTimeOfImpact( CastResult& result) { - m_simplexSolver->reset(); /// compute linear and angular velocity for this interval, to interpolate btVector3 linVelA,angVelA,linVelB,angVelB; @@ -53,7 +104,7 @@ bool btContinuousConvexCollision::calcTimeOfImpact( btScalar boundingRadiusA = m_convexA->getAngularMotionDisc(); - btScalar boundingRadiusB = m_convexB->getAngularMotionDisc(); + btScalar boundingRadiusB = m_convexB1?m_convexB1->getAngularMotionDisc():0.f; btScalar maxAngularProjectedVelocity = angVelA.length() * boundingRadiusA + angVelB.length() * boundingRadiusB; btVector3 relLinVel = (linVelB-linVelA); @@ -64,7 +115,6 @@ bool btContinuousConvexCollision::calcTimeOfImpact( return false; - btScalar radius = btScalar(0.001); btScalar lambda = btScalar(0.); btVector3 v(1,0,0); @@ -83,28 +133,14 @@ bool btContinuousConvexCollision::calcTimeOfImpact( //first solution, using GJK - btTransform identityTrans; - identityTrans.setIdentity(); - - btSphereShape raySphere(btScalar(0.0)); - raySphere.setMargin(btScalar(0.)); - - + btScalar radius = 0.001f; // result.drawCoordSystem(sphereTr); btPointCollector pointCollector1; { - - btGjkPairDetector gjk(m_convexA,m_convexB,m_convexA->getShapeType(),m_convexB->getShapeType(),m_convexA->getMargin(),m_convexB->getMargin(),m_simplexSolver,m_penetrationDepthSolver); - btGjkPairDetector::ClosestPointInput input; - //we don't use margins during CCD - // gjk.setIgnoreMargin(true); - - input.m_transformA = fromA; - input.m_transformB = fromB; - gjk.getClosestPoints(input,pointCollector1,0); + computeClosestPoints(fromA,fromB,pointCollector1); hasResult = pointCollector1.m_hasResult; c = pointCollector1.m_pointInWorld; @@ -113,11 +149,12 @@ bool btContinuousConvexCollision::calcTimeOfImpact( if (hasResult) { btScalar dist; - dist = pointCollector1.m_distance; + dist = pointCollector1.m_distance + result.m_allowedPenetration; n = pointCollector1.m_normalOnBInWorld; - btScalar projectedLinearVelocity = relLinVel.dot(n); - + if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=SIMD_EPSILON) + return false; + //not close enough while (dist > radius) { @@ -125,19 +162,10 @@ bool btContinuousConvexCollision::calcTimeOfImpact( { result.m_debugDrawer->drawSphere(c,0.2f,btVector3(1,1,1)); } - numIter++; - if (numIter > maxIter) - { - return false; //todo: report a failure - } btScalar dLambda = btScalar(0.); projectedLinearVelocity = relLinVel.dot(n); - //calculate safe moving fraction from distance / (linear+rotational velocity) - - //btScalar clippedDist = GEN_min(angularConservativeRadius,dist); - //btScalar clippedDist = dist; //don't report time of impact for motion away from the contact normal (or causes minor penetration) if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=SIMD_EPSILON) @@ -182,37 +210,27 @@ bool btContinuousConvexCollision::calcTimeOfImpact( result.DebugDraw( lambda ); btPointCollector pointCollector; - btGjkPairDetector gjk(m_convexA,m_convexB,m_simplexSolver,m_penetrationDepthSolver); - btGjkPairDetector::ClosestPointInput input; - input.m_transformA = interpolatedTransA; - input.m_transformB = interpolatedTransB; - gjk.getClosestPoints(input,pointCollector,0); + computeClosestPoints(interpolatedTransA,interpolatedTransB,pointCollector); + if (pointCollector.m_hasResult) { - if (pointCollector.m_distance < btScalar(0.)) - { - //degenerate ?! - result.m_fraction = lastLambda; - n = pointCollector.m_normalOnBInWorld; - result.m_normal=n;//.setValue(1,1,1);// = n; - result.m_hitPoint = pointCollector.m_pointInWorld; - return true; - } + dist = pointCollector.m_distance+result.m_allowedPenetration; c = pointCollector.m_pointInWorld; n = pointCollector.m_normalOnBInWorld; - dist = pointCollector.m_distance; } else { - //?? + result.reportFailure(-1, numIter); return false; } - + numIter++; + if (numIter > maxIter) + { + result.reportFailure(-2, numIter); + return false; + } } - if ((projectedLinearVelocity+ maxAngularProjectedVelocity)<=result.m_allowedPenetration)//SIMD_EPSILON) - return false; - result.m_fraction = lambda; result.m_normal = n; result.m_hitPoint = c; @@ -221,16 +239,5 @@ bool btContinuousConvexCollision::calcTimeOfImpact( return false; -/* -//todo: - //if movement away from normal, discard result - btVector3 move = transBLocalTo.getOrigin() - transBLocalFrom.getOrigin(); - if (result.m_fraction < btScalar(1.)) - { - if (move.dot(result.m_normal) <= btScalar(0.)) - { - } - } -*/ - } + diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h index 28c2b4d6156..bdc0572f75a 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h @@ -14,13 +14,14 @@ subject to the following restrictions: */ -#ifndef CONTINUOUS_COLLISION_CONVEX_CAST_H -#define CONTINUOUS_COLLISION_CONVEX_CAST_H +#ifndef BT_CONTINUOUS_COLLISION_CONVEX_CAST_H +#define BT_CONTINUOUS_COLLISION_CONVEX_CAST_H #include "btConvexCast.h" #include "btSimplexSolverInterface.h" class btConvexPenetrationDepthSolver; class btConvexShape; +class btStaticPlaneShape; /// btContinuousConvexCollision implements angular and linear time of impact for convex objects. /// Based on Brian Mirtich's Conservative Advancement idea (PhD thesis). @@ -31,13 +32,18 @@ class btContinuousConvexCollision : public btConvexCast btSimplexSolverInterface* m_simplexSolver; btConvexPenetrationDepthSolver* m_penetrationDepthSolver; const btConvexShape* m_convexA; - const btConvexShape* m_convexB; + //second object is either a convex or a plane (code sharing) + const btConvexShape* m_convexB1; + const btStaticPlaneShape* m_planeShape; + void computeClosestPoints( const btTransform& transA, const btTransform& transB,struct btPointCollector& pointCollector); public: btContinuousConvexCollision (const btConvexShape* shapeA,const btConvexShape* shapeB ,btSimplexSolverInterface* simplexSolver,btConvexPenetrationDepthSolver* penetrationDepthSolver); + btContinuousConvexCollision(const btConvexShape* shapeA,const btStaticPlaneShape* plane ); + virtual bool calcTimeOfImpact( const btTransform& fromA, const btTransform& toA, @@ -48,5 +54,6 @@ public: }; -#endif //CONTINUOUS_COLLISION_CONVEX_CAST_H + +#endif //BT_CONTINUOUS_COLLISION_CONVEX_CAST_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h index b0bce341e41..bfd79d03beb 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h @@ -14,8 +14,8 @@ subject to the following restrictions: */ -#ifndef CONVEX_CAST_H -#define CONVEX_CAST_H +#ifndef BT_CONVEX_CAST_H +#define BT_CONVEX_CAST_H #include "LinearMath/btTransform.h" #include "LinearMath/btVector3.h" @@ -39,7 +39,7 @@ public: virtual void DebugDraw(btScalar fraction) {(void)fraction;} virtual void drawCoordSystem(const btTransform& trans) {(void)trans;} - + virtual void reportFailure(int errNo, int numIterations) {(void)errNo;(void)numIterations;} CastResult() :m_fraction(btScalar(BT_LARGE_FLOAT)), m_debugDrawer(0), @@ -70,4 +70,4 @@ public: CastResult& result) = 0; }; -#endif //CONVEX_CAST_H +#endif //BT_CONVEX_CAST_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h index 7e3fde8e291..72eb5aec461 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btConvexPenetrationDepthSolver.h @@ -14,8 +14,8 @@ subject to the following restrictions: */ -#ifndef __CONVEX_PENETRATION_DEPTH_H -#define __CONVEX_PENETRATION_DEPTH_H +#ifndef BT_CONVEX_PENETRATION_DEPTH_H +#define BT_CONVEX_PENETRATION_DEPTH_H class btStackAlloc; class btVector3; @@ -38,5 +38,5 @@ public: }; -#endif //CONVEX_PENETRATION_DEPTH_H +#endif //BT_CONVEX_PENETRATION_DEPTH_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h index bc711ad495c..f958cc523ef 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btDiscreteCollisionDetectorInterface.h @@ -14,8 +14,9 @@ subject to the following restrictions: */ -#ifndef DISCRETE_COLLISION_DETECTOR1_INTERFACE_H -#define DISCRETE_COLLISION_DETECTOR1_INTERFACE_H +#ifndef BT_DISCRETE_COLLISION_DETECTOR1_INTERFACE_H +#define BT_DISCRETE_COLLISION_DETECTOR1_INTERFACE_H + #include "LinearMath/btTransform.h" #include "LinearMath/btVector3.h" class btStackAlloc; @@ -86,4 +87,5 @@ struct btStorageResult : public btDiscreteCollisionDetectorInterface::Result } }; -#endif //DISCRETE_COLLISION_DETECTOR_INTERFACE1_H +#endif //BT_DISCRETE_COLLISION_DETECTOR1_INTERFACE_H + diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h index a977c9e83f7..6a42ee63b03 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h @@ -15,8 +15,8 @@ subject to the following restrictions: -#ifndef GJK_CONVEX_CAST_H -#define GJK_CONVEX_CAST_H +#ifndef BT_GJK_CONVEX_CAST_H +#define BT_GJK_CONVEX_CAST_H #include "BulletCollision/CollisionShapes/btCollisionMargin.h" @@ -47,4 +47,4 @@ public: }; -#endif //GJK_CONVEX_CAST_H +#endif //BT_GJK_CONVEX_CAST_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp index f74261d4b21..3268f06c2f9 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.cpp @@ -511,7 +511,6 @@ namespace gjkepa2_impl { btVector3 n; btScalar d; - btScalar p; sSV* c[3]; sFace* f[3]; sFace* l[2]; @@ -657,7 +656,7 @@ namespace gjkepa2_impl remove(m_hull,best); append(m_stock,best); best=findbest(); - if(best->p>=outer.p) outer=*best; + outer=*best; } else { m_status=eStatus::InvalidHull;break; } } else { m_status=eStatus::AccuraryReached;break; } } else { m_status=eStatus::OutOfVertices;break; } @@ -696,6 +695,42 @@ namespace gjkepa2_impl m_result.p[0]=1; return(m_status); } + bool getedgedist(sFace* face, sSV* a, sSV* b, btScalar& dist) + { + const btVector3 ba = b->w - a->w; + const btVector3 n_ab = btCross(ba, face->n); // Outward facing edge normal direction, on triangle plane + const btScalar a_dot_nab = btDot(a->w, n_ab); // Only care about the sign to determine inside/outside, so not normalization required + + if(a_dot_nab < 0) + { + // Outside of edge a->b + + const btScalar ba_l2 = ba.length2(); + const btScalar a_dot_ba = btDot(a->w, ba); + const btScalar b_dot_ba = btDot(b->w, ba); + + if(a_dot_ba > 0) + { + // Pick distance vertex a + dist = a->w.length(); + } + else if(b_dot_ba < 0) + { + // Pick distance vertex b + dist = b->w.length(); + } + else + { + // Pick distance to edge a->b + const btScalar a_dot_b = btDot(a->w, b->w); + dist = btSqrt(btMax((a->w.length2() * b->w.length2() - a_dot_b * a_dot_b) / ba_l2, (btScalar)0)); + } + + return true; + } + + return false; + } sFace* newface(sSV* a,sSV* b,sSV* c,bool forced) { if(m_stock.root) @@ -710,41 +745,48 @@ namespace gjkepa2_impl face->n = btCross(b->w-a->w,c->w-a->w); const btScalar l=face->n.length(); const bool v=l>EPA_ACCURACY; - face->p = btMin(btMin( - btDot(a->w,btCross(face->n,a->w-b->w)), - btDot(b->w,btCross(face->n,b->w-c->w))), - btDot(c->w,btCross(face->n,c->w-a->w))) / - (v?l:1); - face->p = face->p>=-EPA_INSIDE_EPS?0:face->p; + if(v) { - face->d = btDot(a->w,face->n)/l; - face->n /= l; - if(forced||(face->d>=-EPA_PLANE_EPS)) + if(!(getedgedist(face, a, b, face->d) || + getedgedist(face, b, c, face->d) || + getedgedist(face, c, a, face->d))) + { + // Origin projects to the interior of the triangle + // Use distance to triangle plane + face->d = btDot(a->w, face->n) / l; + } + + face->n /= l; + if(forced || (face->d >= -EPA_PLANE_EPS)) { - return(face); - } else m_status=eStatus::NonConvex; - } else m_status=eStatus::Degenerated; - remove(m_hull,face); - append(m_stock,face); - return(0); + return face; + } + else + m_status=eStatus::NonConvex; + } + else + m_status=eStatus::Degenerated; + + remove(m_hull, face); + append(m_stock, face); + return 0; + } - m_status=m_stock.root?eStatus::OutOfVertices:eStatus::OutOfFaces; - return(0); + m_status = m_stock.root ? eStatus::OutOfVertices : eStatus::OutOfFaces; + return 0; } sFace* findbest() { sFace* minf=m_hull.root; btScalar mind=minf->d*minf->d; - btScalar maxp=minf->p; for(sFace* f=minf->l[1];f;f=f->l[1]) { const btScalar sqd=f->d*f->d; - if((f->p>=maxp)&&(sqd<mind)) + if(sqd<mind) { minf=f; mind=sqd; - maxp=f->p; } } return(minf); diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h index 2296527d7db..ac501d5ecfe 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkEpa2.h @@ -22,8 +22,9 @@ misrepresented as being the original software. /* GJK-EPA collision solver by Nathanael Presson, 2008 */ -#ifndef _68DA1F85_90B7_4bb0_A705_83B4040A75C6_ -#define _68DA1F85_90B7_4bb0_A705_83B4040A75C6_ +#ifndef BT_GJK_EPA2_H +#define BT_GJK_EPA2_H + #include "BulletCollision/CollisionShapes/btConvexShape.h" ///btGjkEpaSolver contributed under zlib by Nathanael Presson @@ -70,4 +71,5 @@ static bool SignedDistance( const btConvexShape* shape0,const btTransform& wtrs }; -#endif +#endif //BT_GJK_EPA2_H + diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h index cc6287c86b0..2277a19d981 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h @@ -16,8 +16,8 @@ subject to the following restrictions: -#ifndef GJK_PAIR_DETECTOR_H -#define GJK_PAIR_DETECTOR_H +#ifndef BT_GJK_PAIR_DETECTOR_H +#define BT_GJK_PAIR_DETECTOR_H #include "btDiscreteCollisionDetectorInterface.h" #include "BulletCollision/CollisionShapes/btCollisionMargin.h" @@ -100,4 +100,4 @@ public: }; -#endif //GJK_PAIR_DETECTOR_H +#endif //BT_GJK_PAIR_DETECTOR_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h index cd310570e06..0ce9dd25926 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h @@ -13,14 +13,14 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef MANIFOLD_CONTACT_POINT_H -#define MANIFOLD_CONTACT_POINT_H +#ifndef BT_MANIFOLD_CONTACT_POINT_H +#define BT_MANIFOLD_CONTACT_POINT_H #include "LinearMath/btVector3.h" #include "LinearMath/btTransformUtil.h" #ifdef PFX_USE_FREE_VECTORMATH - #include "physics_effects\base_level\solver\pfx_constraint_row.h" + #include "physics_effects/base_level/solver/pfx_constraint_row.h" typedef sce::PhysicsEffects::PfxConstraintRow btConstraintRow; #else // Don't change following order of parameters @@ -155,4 +155,4 @@ class btManifoldPoint }; -#endif //MANIFOLD_CONTACT_POINT_H +#endif //BT_MANIFOLD_CONTACT_POINT_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h index 7b6c8a63779..6a8fe52f36f 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btMinkowskiPenetrationDepthSolver.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef MINKOWSKI_PENETRATION_DEPTH_SOLVER_H -#define MINKOWSKI_PENETRATION_DEPTH_SOLVER_H +#ifndef BT_MINKOWSKI_PENETRATION_DEPTH_SOLVER_H +#define BT_MINKOWSKI_PENETRATION_DEPTH_SOLVER_H #include "btConvexPenetrationDepthSolver.h" @@ -36,5 +36,5 @@ public: ); }; -#endif //MINKOWSKI_PENETRATION_DEPTH_SOLVER_H +#endif //BT_MINKOWSKI_PENETRATION_DEPTH_SOLVER_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp index 924a8af87d1..954b8395299 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp @@ -21,7 +21,9 @@ subject to the following restrictions: btScalar gContactBreakingThreshold = btScalar(0.02); ContactDestroyedCallback gContactDestroyedCallback = 0; ContactProcessedCallback gContactProcessedCallback = 0; - +///gContactCalcArea3Points will approximate the convex hull area using 3 points +///when setting it to false, it will use 4 points to compute the area: it is more accurate but slower +bool gContactCalcArea3Points = true; btPersistentManifold::btPersistentManifold() @@ -84,10 +86,28 @@ void btPersistentManifold::clearUserCache(btManifoldPoint& pt) } +static inline btScalar calcArea4Points(const btVector3 &p0,const btVector3 &p1,const btVector3 &p2,const btVector3 &p3) +{ + // It calculates possible 3 area constructed from random 4 points and returns the biggest one. + + btVector3 a[3],b[3]; + a[0] = p0 - p1; + a[1] = p0 - p2; + a[2] = p0 - p3; + b[0] = p2 - p3; + b[1] = p1 - p3; + b[2] = p1 - p2; + + //todo: Following 3 cross production can be easily optimized by SIMD. + btVector3 tmp0 = a[0].cross(b[0]); + btVector3 tmp1 = a[1].cross(b[1]); + btVector3 tmp2 = a[2].cross(b[2]); + + return btMax(btMax(tmp0.length2(),tmp1.length2()),tmp2.length2()); +} int btPersistentManifold::sortCachedPoints(const btManifoldPoint& pt) { - //calculate 4 possible cases areas, and take biggest area //also need to keep 'deepest' @@ -106,6 +126,9 @@ int btPersistentManifold::sortCachedPoints(const btManifoldPoint& pt) #endif //KEEP_DEEPEST_POINT btScalar res0(btScalar(0.)),res1(btScalar(0.)),res2(btScalar(0.)),res3(btScalar(0.)); + + if (gContactCalcArea3Points) + { if (maxPenetrationIndex != 0) { btVector3 a0 = pt.m_localPointA-m_pointCache[1].m_localPointA; @@ -136,10 +159,29 @@ int btPersistentManifold::sortCachedPoints(const btManifoldPoint& pt) btVector3 cross = a3.cross(b3); res3 = cross.length2(); } + } + else + { + if(maxPenetrationIndex != 0) { + res0 = calcArea4Points(pt.m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[2].m_localPointA,m_pointCache[3].m_localPointA); + } - btVector4 maxvec(res0,res1,res2,res3); - int biggestarea = maxvec.closestAxis4(); - return biggestarea; + if(maxPenetrationIndex != 1) { + res1 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[2].m_localPointA,m_pointCache[3].m_localPointA); + } + + if(maxPenetrationIndex != 2) { + res2 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[3].m_localPointA); + } + + if(maxPenetrationIndex != 3) { + res3 = calcArea4Points(pt.m_localPointA,m_pointCache[0].m_localPointA,m_pointCache[1].m_localPointA,m_pointCache[2].m_localPointA); + } + } + btVector4 maxvec(res0,res1,res2,res3); + int biggestarea = maxvec.closestAxis4(); + return biggestarea; + } diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h index c8aac637307..d877f09944f 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef PERSISTENT_MANIFOLD_H -#define PERSISTENT_MANIFOLD_H +#ifndef BT_PERSISTENT_MANIFOLD_H +#define BT_PERSISTENT_MANIFOLD_H #include "LinearMath/btVector3.h" @@ -197,14 +197,10 @@ public: #endif } + bool validContactDistance(const btManifoldPoint& pt) const { - if (pt.m_lifeTime >1) - { - return pt.m_distance1 <= getContactBreakingThreshold(); - } - return pt.m_distance1 <= getContactProcessingThreshold(); - + return pt.m_distance1 <= getContactBreakingThreshold(); } /// calculated new worldspace coordinates and depth, and reject points that exceed the collision margin void refreshContactPoints( const btTransform& trA,const btTransform& trB); @@ -229,4 +225,4 @@ public: -#endif //PERSISTENT_MANIFOLD_H +#endif //BT_PERSISTENT_MANIFOLD_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h index 6ca60548e71..18da171011a 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPointCollector.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef POINT_COLLECTOR_H -#define POINT_COLLECTOR_H +#ifndef BT_POINT_COLLECTOR_H +#define BT_POINT_COLLECTOR_H #include "btDiscreteCollisionDetectorInterface.h" @@ -60,5 +60,5 @@ struct btPointCollector : public btDiscreteCollisionDetectorInterface::Result } }; -#endif //POINT_COLLECTOR_H +#endif //BT_POINT_COLLECTOR_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp new file mode 100644 index 00000000000..db1909113b3 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.cpp @@ -0,0 +1,440 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2011 Advanced Micro Devices, Inc. http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +///This file was written by Erwin Coumans +///Separating axis rest based on work from Pierre Terdiman, see +///And contact clipping based on work from Simon Hobbs + + +#include "btPolyhedralContactClipping.h" +#include "BulletCollision/CollisionShapes/btConvexPolyhedron.h" + +#include <float.h> //for FLT_MAX + +int gExpectedNbTests=0; +int gActualNbTests = 0; +bool gUseInternalObject = true; + +// Clips a face to the back of a plane +void btPolyhedralContactClipping::clipFace(const btVertexArray& pVtxIn, btVertexArray& ppVtxOut, const btVector3& planeNormalWS,btScalar planeEqWS) +{ + + int ve; + btScalar ds, de; + int numVerts = pVtxIn.size(); + if (numVerts < 2) + return; + + btVector3 firstVertex=pVtxIn[pVtxIn.size()-1]; + btVector3 endVertex = pVtxIn[0]; + + ds = planeNormalWS.dot(firstVertex)+planeEqWS; + + for (ve = 0; ve < numVerts; ve++) + { + endVertex=pVtxIn[ve]; + + de = planeNormalWS.dot(endVertex)+planeEqWS; + + if (ds<0) + { + if (de<0) + { + // Start < 0, end < 0, so output endVertex + ppVtxOut.push_back(endVertex); + } + else + { + // Start < 0, end >= 0, so output intersection + ppVtxOut.push_back( firstVertex.lerp(endVertex,btScalar(ds * 1.f/(ds - de)))); + } + } + else + { + if (de<0) + { + // Start >= 0, end < 0 so output intersection and end + ppVtxOut.push_back(firstVertex.lerp(endVertex,btScalar(ds * 1.f/(ds - de)))); + ppVtxOut.push_back(endVertex); + } + } + firstVertex = endVertex; + ds = de; + } +} + + +static bool TestSepAxis(const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btVector3& sep_axis, btScalar& depth) +{ + btScalar Min0,Max0; + btScalar Min1,Max1; + hullA.project(transA,sep_axis, Min0, Max0); + hullB.project(transB, sep_axis, Min1, Max1); + + if(Max0<Min1 || Max1<Min0) + return false; + + btScalar d0 = Max0 - Min1; + assert(d0>=0.0f); + btScalar d1 = Max1 - Min0; + assert(d1>=0.0f); + depth = d0<d1 ? d0:d1; + return true; +} + + + +static int gActualSATPairTests=0; + +inline bool IsAlmostZero(const btVector3& v) +{ + if(fabsf(v.x())>1e-6 || fabsf(v.y())>1e-6 || fabsf(v.z())>1e-6) return false; + return true; +} + +#ifdef TEST_INTERNAL_OBJECTS + +inline void BoxSupport(const btScalar extents[3], const btScalar sv[3], btScalar p[3]) +{ + // This version is ~11.000 cycles (4%) faster overall in one of the tests. +// IR(p[0]) = IR(extents[0])|(IR(sv[0])&SIGN_BITMASK); +// IR(p[1]) = IR(extents[1])|(IR(sv[1])&SIGN_BITMASK); +// IR(p[2]) = IR(extents[2])|(IR(sv[2])&SIGN_BITMASK); + p[0] = sv[0] < 0.0f ? -extents[0] : extents[0]; + p[1] = sv[1] < 0.0f ? -extents[1] : extents[1]; + p[2] = sv[2] < 0.0f ? -extents[2] : extents[2]; +} + +void InverseTransformPoint3x3(btVector3& out, const btVector3& in, const btTransform& tr) +{ + const btMatrix3x3& rot = tr.getBasis(); + const btVector3& r0 = rot[0]; + const btVector3& r1 = rot[1]; + const btVector3& r2 = rot[2]; + + const btScalar x = r0.x()*in.x() + r1.x()*in.y() + r2.x()*in.z(); + const btScalar y = r0.y()*in.x() + r1.y()*in.y() + r2.y()*in.z(); + const btScalar z = r0.z()*in.x() + r1.z()*in.y() + r2.z()*in.z(); + + out.setValue(x, y, z); +} + + bool TestInternalObjects( const btTransform& trans0, const btTransform& trans1, const btVector3& delta_c, const btVector3& axis, const btConvexPolyhedron& convex0, const btConvexPolyhedron& convex1, btScalar dmin) +{ + const btScalar dp = delta_c.dot(axis); + + btVector3 localAxis0; + InverseTransformPoint3x3(localAxis0, axis,trans0); + btVector3 localAxis1; + InverseTransformPoint3x3(localAxis1, axis,trans1); + + btScalar p0[3]; + BoxSupport(convex0.m_extents, localAxis0, p0); + btScalar p1[3]; + BoxSupport(convex1.m_extents, localAxis1, p1); + + const btScalar Radius0 = p0[0]*localAxis0.x() + p0[1]*localAxis0.y() + p0[2]*localAxis0.z(); + const btScalar Radius1 = p1[0]*localAxis1.x() + p1[1]*localAxis1.y() + p1[2]*localAxis1.z(); + + const btScalar MinRadius = Radius0>convex0.m_radius ? Radius0 : convex0.m_radius; + const btScalar MaxRadius = Radius1>convex1.m_radius ? Radius1 : convex1.m_radius; + + const btScalar MinMaxRadius = MaxRadius + MinRadius; + const btScalar d0 = MinMaxRadius + dp; + const btScalar d1 = MinMaxRadius - dp; + + const btScalar depth = d0<d1 ? d0:d1; + if(depth>dmin) + return false; + return true; +} +#endif //TEST_INTERNAL_OBJECTS + + +bool btPolyhedralContactClipping::findSeparatingAxis( const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep) +{ + gActualSATPairTests++; + +//#ifdef TEST_INTERNAL_OBJECTS + const btVector3 c0 = transA * hullA.m_localCenter; + const btVector3 c1 = transB * hullB.m_localCenter; + const btVector3 DeltaC2 = c0 - c1; +//#endif + + btScalar dmin = FLT_MAX; + int curPlaneTests=0; + + int numFacesA = hullA.m_faces.size(); + // Test normals from hullA + for(int i=0;i<numFacesA;i++) + { + const btVector3 Normal(hullA.m_faces[i].m_plane[0], hullA.m_faces[i].m_plane[1], hullA.m_faces[i].m_plane[2]); + const btVector3 faceANormalWS = transA.getBasis() * Normal; + if (DeltaC2.dot(faceANormalWS)<0) + continue; + + curPlaneTests++; +#ifdef TEST_INTERNAL_OBJECTS + gExpectedNbTests++; + if(gUseInternalObject && !TestInternalObjects(transA,transB, DeltaC2, faceANormalWS, hullA, hullB, dmin)) + continue; + gActualNbTests++; +#endif + + btScalar d; + if(!TestSepAxis( hullA, hullB, transA,transB, faceANormalWS, d)) + return false; + + if(d<dmin) + { + dmin = d; + sep = faceANormalWS; + } + } + + int numFacesB = hullB.m_faces.size(); + // Test normals from hullB + for(int i=0;i<numFacesB;i++) + { + const btVector3 Normal(hullB.m_faces[i].m_plane[0], hullB.m_faces[i].m_plane[1], hullB.m_faces[i].m_plane[2]); + const btVector3 WorldNormal = transB.getBasis() * Normal; + if (DeltaC2.dot(WorldNormal)<0) + continue; + + curPlaneTests++; +#ifdef TEST_INTERNAL_OBJECTS + gExpectedNbTests++; + if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, WorldNormal, hullA, hullB, dmin)) + continue; + gActualNbTests++; +#endif + + btScalar d; + if(!TestSepAxis(hullA, hullB,transA,transB, WorldNormal,d)) + return false; + + if(d<dmin) + { + dmin = d; + sep = WorldNormal; + } + } + + btVector3 edgeAstart,edgeAend,edgeBstart,edgeBend; + + int curEdgeEdge = 0; + // Test edges + for(int e0=0;e0<hullA.m_uniqueEdges.size();e0++) + { + const btVector3 edge0 = hullA.m_uniqueEdges[e0]; + const btVector3 WorldEdge0 = transA.getBasis() * edge0; + for(int e1=0;e1<hullB.m_uniqueEdges.size();e1++) + { + const btVector3 edge1 = hullB.m_uniqueEdges[e1]; + const btVector3 WorldEdge1 = transB.getBasis() * edge1; + + btVector3 Cross = WorldEdge0.cross(WorldEdge1); + curEdgeEdge++; + if(!IsAlmostZero(Cross)) + { + Cross = Cross.normalize(); + if (DeltaC2.dot(Cross)<0) + continue; + + +#ifdef TEST_INTERNAL_OBJECTS + gExpectedNbTests++; + if(gUseInternalObject && !TestInternalObjects(transA,transB,DeltaC2, Cross, hullA, hullB, dmin)) + continue; + gActualNbTests++; +#endif + + btScalar dist; + if(!TestSepAxis( hullA, hullB, transA,transB, Cross, dist)) + return false; + + if(dist<dmin) + { + dmin = dist; + sep = Cross; + } + } + } + + } + + const btVector3 deltaC = transB.getOrigin() - transA.getOrigin(); + if((deltaC.dot(sep))>0.0f) + sep = -sep; + + return true; +} + +void btPolyhedralContactClipping::clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btTransform& transA, btVertexArray& worldVertsB1, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut) +{ + btVertexArray worldVertsB2; + btVertexArray* pVtxIn = &worldVertsB1; + btVertexArray* pVtxOut = &worldVertsB2; + pVtxOut->reserve(pVtxIn->size()); + + int closestFaceA=-1; + { + btScalar dmin = FLT_MAX; + for(int face=0;face<hullA.m_faces.size();face++) + { + const btVector3 Normal(hullA.m_faces[face].m_plane[0], hullA.m_faces[face].m_plane[1], hullA.m_faces[face].m_plane[2]); + const btVector3 faceANormalWS = transA.getBasis() * Normal; + + btScalar d = faceANormalWS.dot(separatingNormal); + if (d < dmin) + { + dmin = d; + closestFaceA = face; + } + } + } + if (closestFaceA<0) + return; + + const btFace& polyA = hullA.m_faces[closestFaceA]; + + // clip polygon to back of planes of all faces of hull A that are adjacent to witness face + int numContacts = pVtxIn->size(); + int numVerticesA = polyA.m_indices.size(); + for(int e0=0;e0<numVerticesA;e0++) + { + const btVector3& a = hullA.m_vertices[polyA.m_indices[e0]]; + const btVector3& b = hullA.m_vertices[polyA.m_indices[(e0+1)%numVerticesA]]; + const btVector3 edge0 = a - b; + const btVector3 WorldEdge0 = transA.getBasis() * edge0; + btVector3 worldPlaneAnormal1 = transA.getBasis()* btVector3(polyA.m_plane[0],polyA.m_plane[1],polyA.m_plane[2]); + + btVector3 planeNormalWS1 = -WorldEdge0.cross(worldPlaneAnormal1);//.cross(WorldEdge0); + btVector3 worldA1 = transA*a; + btScalar planeEqWS1 = -worldA1.dot(planeNormalWS1); + +//int otherFace=0; +#ifdef BLA1 + int otherFace = polyA.m_connectedFaces[e0]; + btVector3 localPlaneNormal (hullA.m_faces[otherFace].m_plane[0],hullA.m_faces[otherFace].m_plane[1],hullA.m_faces[otherFace].m_plane[2]); + btScalar localPlaneEq = hullA.m_faces[otherFace].m_plane[3]; + + btVector3 planeNormalWS = transA.getBasis()*localPlaneNormal; + btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin()); +#else + btVector3 planeNormalWS = planeNormalWS1; + btScalar planeEqWS=planeEqWS1; + +#endif + //clip face + + clipFace(*pVtxIn, *pVtxOut,planeNormalWS,planeEqWS); + btSwap(pVtxIn,pVtxOut); + pVtxOut->resize(0); + } + + + +//#define ONLY_REPORT_DEEPEST_POINT + + btVector3 point; + + + // only keep points that are behind the witness face + { + btVector3 localPlaneNormal (polyA.m_plane[0],polyA.m_plane[1],polyA.m_plane[2]); + btScalar localPlaneEq = polyA.m_plane[3]; + btVector3 planeNormalWS = transA.getBasis()*localPlaneNormal; + btScalar planeEqWS=localPlaneEq-planeNormalWS.dot(transA.getOrigin()); + for (int i=0;i<pVtxIn->size();i++) + { + + btScalar depth = planeNormalWS.dot(pVtxIn->at(i))+planeEqWS; + if (depth <=minDist) + { +// printf("clamped: depth=%f to minDist=%f\n",depth,minDist); + depth = minDist; + } + + if (depth <=maxDist) + { + btVector3 point = pVtxIn->at(i); +#ifdef ONLY_REPORT_DEEPEST_POINT + curMaxDist = depth; +#else +#if 0 + if (depth<-3) + { + printf("error in btPolyhedralContactClipping depth = %f\n", depth); + printf("likely wrong separatingNormal passed in\n"); + } +#endif + resultOut.addContactPoint(separatingNormal,point,depth); +#endif + } + } + } +#ifdef ONLY_REPORT_DEEPEST_POINT + if (curMaxDist<maxDist) + { + resultOut.addContactPoint(separatingNormal,point,curMaxDist); + } +#endif //ONLY_REPORT_DEEPEST_POINT + +} + + +void btPolyhedralContactClipping::clipHullAgainstHull(const btVector3& separatingNormal1, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut) +{ + + btVector3 separatingNormal = separatingNormal1.normalized(); + const btVector3 c0 = transA * hullA.m_localCenter; + const btVector3 c1 = transB * hullB.m_localCenter; + const btVector3 DeltaC2 = c0 - c1; + + + btScalar curMaxDist=maxDist; + int closestFaceB=-1; + btScalar dmax = -FLT_MAX; + { + for(int face=0;face<hullB.m_faces.size();face++) + { + const btVector3 Normal(hullB.m_faces[face].m_plane[0], hullB.m_faces[face].m_plane[1], hullB.m_faces[face].m_plane[2]); + const btVector3 WorldNormal = transB.getBasis() * Normal; + btScalar d = WorldNormal.dot(separatingNormal); + if (d > dmax) + { + dmax = d; + closestFaceB = face; + } + } + } + btVertexArray worldVertsB1; + { + const btFace& polyB = hullB.m_faces[closestFaceB]; + const int numVertices = polyB.m_indices.size(); + for(int e0=0;e0<numVertices;e0++) + { + const btVector3& b = hullB.m_vertices[polyB.m_indices[e0]]; + worldVertsB1.push_back(transB*b); + } + } + + + if (closestFaceB>=0) + clipFaceAgainstHull(separatingNormal, hullA, transA,worldVertsB1, minDist, maxDist,resultOut); + +} diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h new file mode 100644 index 00000000000..99103df2027 --- /dev/null +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btPolyhedralContactClipping.h @@ -0,0 +1,46 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2011 Advanced Micro Devices, Inc. http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +///This file was written by Erwin Coumans + + +#ifndef BT_POLYHEDRAL_CONTACT_CLIPPING_H +#define BT_POLYHEDRAL_CONTACT_CLIPPING_H + + +#include "LinearMath/btAlignedObjectArray.h" +#include "LinearMath/btTransform.h" +#include "btDiscreteCollisionDetectorInterface.h" + +class btConvexPolyhedron; + +typedef btAlignedObjectArray<btVector3> btVertexArray; + +// Clips a face to the back of a plane +struct btPolyhedralContactClipping +{ + static void clipHullAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, const btScalar minDist, btScalar maxDist, btDiscreteCollisionDetectorInterface::Result& resultOut); + static void clipFaceAgainstHull(const btVector3& separatingNormal, const btConvexPolyhedron& hullA, const btTransform& transA, btVertexArray& worldVertsB1, const btScalar minDist, btScalar maxDist,btDiscreteCollisionDetectorInterface::Result& resultOut); + + static bool findSeparatingAxis( const btConvexPolyhedron& hullA, const btConvexPolyhedron& hullB, const btTransform& transA,const btTransform& transB, btVector3& sep); + + ///the clipFace method is used internally + static void clipFace(const btVertexArray& pVtxIn, btVertexArray& ppVtxOut, const btVector3& planeNormalWS,btScalar planeEqWS); + +}; + +#endif // BT_POLYHEDRAL_CONTACT_CLIPPING_H + diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h index bdd1add36d2..f012889a70e 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btRaycastCallback.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef RAYCAST_TRI_CALLBACK_H -#define RAYCAST_TRI_CALLBACK_H +#ifndef BT_RAYCAST_TRI_CALLBACK_H +#define BT_RAYCAST_TRI_CALLBACK_H #include "BulletCollision/CollisionShapes/btTriangleCallback.h" #include "LinearMath/btTransform.h" @@ -68,5 +68,5 @@ public: virtual btScalar reportHit (const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex) = 0; }; -#endif //RAYCAST_TRI_CALLBACK_H +#endif //BT_RAYCAST_TRI_CALLBACK_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h index 823b4e7158b..da8a13914c9 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSimplexSolverInterface.h @@ -15,8 +15,8 @@ subject to the following restrictions: -#ifndef SIMPLEX_SOLVER_INTERFACE_H -#define SIMPLEX_SOLVER_INTERFACE_H +#ifndef BT_SIMPLEX_SOLVER_INTERFACE_H +#define BT_SIMPLEX_SOLVER_INTERFACE_H #include "LinearMath/btVector3.h" @@ -59,5 +59,5 @@ class btSimplexSolverInterface }; #endif -#endif //SIMPLEX_SOLVER_INTERFACE_H +#endif //BT_SIMPLEX_SOLVER_INTERFACE_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h index 05662db5d23..6c8127983eb 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h @@ -14,8 +14,8 @@ subject to the following restrictions: */ -#ifndef SUBSIMPLEX_CONVEX_CAST_H -#define SUBSIMPLEX_CONVEX_CAST_H +#ifndef BT_SUBSIMPLEX_CONVEX_CAST_H +#define BT_SUBSIMPLEX_CONVEX_CAST_H #include "btConvexCast.h" #include "btSimplexSolverInterface.h" @@ -47,4 +47,4 @@ public: }; -#endif //SUBSIMPLEX_CONVEX_CAST_H +#endif //BT_SUBSIMPLEX_CONVEX_CAST_H diff --git a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h index 9a4f552924c..f1c7613efa1 100644 --- a/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h +++ b/extern/bullet2/src/BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h @@ -15,8 +15,8 @@ subject to the following restrictions: -#ifndef btVoronoiSimplexSolver_H -#define btVoronoiSimplexSolver_H +#ifndef BT_VORONOI_SIMPLEX_SOLVER_H +#define BT_VORONOI_SIMPLEX_SOLVER_H #include "btSimplexSolverInterface.h" @@ -175,4 +175,5 @@ public: }; -#endif //VoronoiSimplexSolver +#endif //BT_VORONOI_SIMPLEX_SOLVER_H + diff --git a/extern/bullet2/src/BulletDynamics/Character/btCharacterControllerInterface.h b/extern/bullet2/src/BulletDynamics/Character/btCharacterControllerInterface.h index 19373daa241..c81813c92be 100644 --- a/extern/bullet2/src/BulletDynamics/Character/btCharacterControllerInterface.h +++ b/extern/bullet2/src/BulletDynamics/Character/btCharacterControllerInterface.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef CHARACTER_CONTROLLER_INTERFACE_H -#define CHARACTER_CONTROLLER_INTERFACE_H +#ifndef BT_CHARACTER_CONTROLLER_INTERFACE_H +#define BT_CHARACTER_CONTROLLER_INTERFACE_H #include "LinearMath/btVector3.h" #include "BulletDynamics/Dynamics/btActionInterface.h" @@ -42,4 +42,5 @@ public: virtual bool onGround () const = 0; }; -#endif +#endif //BT_CHARACTER_CONTROLLER_INTERFACE_H + diff --git a/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.h b/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.h index 704355c0ae1..ef01f8a3e60 100644 --- a/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.h +++ b/extern/bullet2/src/BulletDynamics/Character/btKinematicCharacterController.h @@ -14,8 +14,8 @@ subject to the following restrictions: */ -#ifndef KINEMATIC_CHARACTER_CONTROLLER_H -#define KINEMATIC_CHARACTER_CONTROLLER_H +#ifndef BT_KINEMATIC_CHARACTER_CONTROLLER_H +#define BT_KINEMATIC_CHARACTER_CONTROLLER_H #include "LinearMath/btVector3.h" @@ -25,6 +25,7 @@ subject to the following restrictions: class btCollisionShape; +class btConvexShape; class btRigidBody; class btCollisionWorld; class btCollisionDispatcher; @@ -159,4 +160,4 @@ public: bool onGround () const; }; -#endif // KINEMATIC_CHARACTER_CONTROLLER_H +#endif // BT_KINEMATIC_CHARACTER_CONTROLLER_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp index bc371e4062d..755544f0dee 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp @@ -638,6 +638,11 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr btTransform trDeltaAB = trB * trPose * trA.inverse(); btQuaternion qDeltaAB = trDeltaAB.getRotation(); btVector3 swingAxis = btVector3(qDeltaAB.x(), qDeltaAB.y(), qDeltaAB.z()); + float swingAxisLen2 = swingAxis.length2(); + if(btFuzzyZero(swingAxisLen2)) + { + return; + } m_swingAxis = swingAxis; m_swingAxis.normalize(); m_swingCorrection = qDeltaAB.getAngle(); @@ -918,7 +923,7 @@ void btConeTwistConstraint::computeTwistLimitInfo(const btQuaternion& qTwist, if (twistAngle > SIMD_PI) // long way around. flip quat and recalculate. { - qMinTwist = operator-(qTwist); + qMinTwist = -(qTwist); twistAngle = qMinTwist.getAngle(); } if (twistAngle < 0) diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h index c1c5aa851b6..868e62f063e 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h @@ -33,8 +33,8 @@ and swing 1 and 2 are along the z and y axes respectively. -#ifndef CONETWISTCONSTRAINT_H -#define CONETWISTCONSTRAINT_H +#ifndef BT_CONETWISTCONSTRAINT_H +#define BT_CONETWISTCONSTRAINT_H #include "LinearMath/btVector3.h" #include "btJacobianEntry.h" @@ -343,4 +343,4 @@ SIMD_FORCE_INLINE const char* btConeTwistConstraint::serialize(void* dataBuffer, } -#endif //CONETWISTCONSTRAINT_H +#endif //BT_CONETWISTCONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h index 7a8e9c1953d..6f673102be2 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef CONSTRAINT_SOLVER_H -#define CONSTRAINT_SOLVER_H +#ifndef BT_CONSTRAINT_SOLVER_H +#define BT_CONSTRAINT_SOLVER_H #include "LinearMath/btScalar.h" @@ -49,4 +49,4 @@ public: -#endif //CONSTRAINT_SOLVER_H +#endif //BT_CONSTRAINT_SOLVER_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp index d97096d9f26..88859182925 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp @@ -68,9 +68,53 @@ void btContactConstraint::buildJacobian() #include "LinearMath/btMinMax.h" #include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h" -#define ASSERT2 btAssert -#define USE_INTERNAL_APPLY_IMPULSE 1 + +//response between two dynamic objects without friction, assuming 0 penetration depth +btScalar resolveSingleCollision( + btRigidBody* body1, + btCollisionObject* colObj2, + const btVector3& contactPositionWorld, + const btVector3& contactNormalOnB, + const btContactSolverInfo& solverInfo, + btScalar distance) +{ + btRigidBody* body2 = btRigidBody::upcast(colObj2); + + + const btVector3& normal = contactNormalOnB; + + btVector3 rel_pos1 = contactPositionWorld - body1->getWorldTransform().getOrigin(); + btVector3 rel_pos2 = contactPositionWorld - colObj2->getWorldTransform().getOrigin(); + + btVector3 vel1 = body1->getVelocityInLocalPoint(rel_pos1); + btVector3 vel2 = body2? body2->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0); + btVector3 vel = vel1 - vel2; + btScalar rel_vel; + rel_vel = normal.dot(vel); + + btScalar combinedRestitution = body1->getRestitution() * colObj2->getRestitution(); + btScalar restitution = combinedRestitution* -rel_vel; + + btScalar positionalError = solverInfo.m_erp *-distance /solverInfo.m_timeStep ; + btScalar velocityError = -(1.0f + restitution) * rel_vel;// * damping; + btScalar denom0 = body1->computeImpulseDenominator(contactPositionWorld,normal); + btScalar denom1 = body2? body2->computeImpulseDenominator(contactPositionWorld,normal) : 0.f; + btScalar relaxation = 1.f; + btScalar jacDiagABInv = relaxation/(denom0+denom1); + + btScalar penetrationImpulse = positionalError * jacDiagABInv; + btScalar velocityImpulse = velocityError * jacDiagABInv; + + btScalar normalImpulse = penetrationImpulse+velocityImpulse; + normalImpulse = 0.f > normalImpulse ? 0.f: normalImpulse; + + body1->applyImpulse(normal*(normalImpulse), rel_pos1); + if (body2) + body2->applyImpulse(-normal*(normalImpulse), rel_pos2); + + return normalImpulse; +} //bilateral constraint between two dynamic objects @@ -83,7 +127,7 @@ void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1, btScalar normalLenSqr = normal.length2(); - ASSERT2(btFabs(normalLenSqr) < btScalar(1.1)); + btAssert(btFabs(normalLenSqr) < btScalar(1.1)); if (normalLenSqr > btScalar(1.1)) { impulse = btScalar(0.); diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h index 63c1a417bc1..477c79d1756 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef CONTACT_CONSTRAINT_H -#define CONTACT_CONSTRAINT_H +#ifndef BT_CONTACT_CONSTRAINT_H +#define BT_CONTACT_CONSTRAINT_H #include "LinearMath/btVector3.h" #include "btJacobianEntry.h" @@ -57,6 +57,9 @@ public: }; +///very basic collision resolution without friction +btScalar resolveSingleCollision(btRigidBody* body1, class btCollisionObject* colObj2, const btVector3& contactPositionWorld,const btVector3& contactNormalOnB, const struct btContactSolverInfo& solverInfo,btScalar distance); + ///resolveSingleBilateral is an obsolete methods used for vehicle friction between two dynamic objects void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1, @@ -65,4 +68,4 @@ void resolveSingleBilateral(btRigidBody& body1, const btVector3& pos1, -#endif //CONTACT_CONSTRAINT_H +#endif //BT_CONTACT_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index db5bb5e4c51..6204cb3d16c 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef CONTACT_SOLVER_INFO -#define CONTACT_SOLVER_INFO +#ifndef BT_CONTACT_SOLVER_INFO +#define BT_CONTACT_SOLVER_INFO enum btSolverMode { @@ -84,4 +84,4 @@ struct btContactSolverInfo : public btContactSolverInfoData } }; -#endif //CONTACT_SOLVER_INFO +#endif //BT_CONTACT_SOLVER_INFO diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp index 7c5e4f6e7b2..8ff9940bba3 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @@ -130,12 +130,20 @@ int btRotationalLimitMotor::testLimitValue(btScalar test_value) { m_currentLimit = 1;//low limit violation m_currentLimitError = test_value - m_loLimit; + if(m_currentLimitError>SIMD_PI) + m_currentLimitError-=SIMD_2_PI; + else if(m_currentLimitError<-SIMD_PI) + m_currentLimitError+=SIMD_2_PI; return 1; } else if (test_value> m_hiLimit) { m_currentLimit = 2;//High limit violation m_currentLimitError = test_value - m_hiLimit; + if(m_currentLimitError>SIMD_PI) + m_currentLimitError-=SIMD_2_PI; + else if(m_currentLimitError<-SIMD_PI) + m_currentLimitError+=SIMD_2_PI; return 2; }; @@ -1067,4 +1075,4 @@ void btGeneric6DofConstraint::setAxis(const btVector3& axis1,const btVector3& ax m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW; calculateTransforms(); -}
\ No newline at end of file +} diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h index ab745ca5ad6..a8e7bc22902 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h @@ -24,8 +24,8 @@ http://gimpact.sf.net */ -#ifndef GENERIC_6DOF_CONSTRAINT_H -#define GENERIC_6DOF_CONSTRAINT_H +#ifndef BT_GENERIC_6DOF_CONSTRAINT_H +#define BT_GENERIC_6DOF_CONSTRAINT_H #include "LinearMath/btVector3.h" #include "btJacobianEntry.h" @@ -611,4 +611,4 @@ SIMD_FORCE_INLINE const char* btGeneric6DofConstraint::serialize(void* dataBuffe -#endif //GENERIC_6DOF_CONSTRAINT_H +#endif //BT_GENERIC_6DOF_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp index d3503456625..2b38714987b 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp @@ -21,6 +21,19 @@ subject to the following restrictions: btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA) : btGeneric6DofConstraint(rbA, rbB, frameInA, frameInB, useLinearReferenceFrameA) { + init(); +} + + +btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB) + : btGeneric6DofConstraint(rbB, frameInB, useLinearReferenceFrameB) +{ + init(); +} + + +void btGeneric6DofSpringConstraint::init() +{ m_objectType = D6_SPRING_CONSTRAINT_TYPE; for(int i = 0; i < 6; i++) diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h index 16ff973e427..31e0cd531ae 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef GENERIC_6DOF_SPRING_CONSTRAINT_H -#define GENERIC_6DOF_SPRING_CONSTRAINT_H +#ifndef BT_GENERIC_6DOF_SPRING_CONSTRAINT_H +#define BT_GENERIC_6DOF_SPRING_CONSTRAINT_H #include "LinearMath/btVector3.h" @@ -39,9 +39,11 @@ protected: btScalar m_equilibriumPoint[6]; btScalar m_springStiffness[6]; btScalar m_springDamping[6]; // between 0 and 1 (1 == no damping) + void init(); void internalUpdateSprings(btConstraintInfo2* info); public: btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA); + btGeneric6DofSpringConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB); void enableSpring(int index, bool onOff); void setStiffness(int index, btScalar stiffness); void setDamping(int index, btScalar damping); @@ -93,5 +95,5 @@ SIMD_FORCE_INLINE const char* btGeneric6DofSpringConstraint::serialize(void* dat return "btGeneric6DofConstraintData"; } -#endif // GENERIC_6DOF_SPRING_CONSTRAINT_H +#endif // BT_GENERIC_6DOF_SPRING_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h index 15fd4a014cc..a76452ddb64 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef HINGE2_CONSTRAINT_H -#define HINGE2_CONSTRAINT_H +#ifndef BT_HINGE2_CONSTRAINT_H +#define BT_HINGE2_CONSTRAINT_H @@ -54,5 +54,5 @@ public: -#endif // HINGE2_CONSTRAINT_H +#endif // BT_HINGE2_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp index 144beef1cd6..9e3a2baeed9 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp @@ -37,15 +37,15 @@ subject to the following restrictions: btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA) :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB), +#ifdef _BT_USE_CENTER_LIMIT_ + m_limit(), +#endif m_angularOnly(false), m_enableAngularMotor(false), m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), m_useReferenceFrameA(useReferenceFrameA), m_flags(0) -#ifdef _BT_USE_CENTER_LIMIT_ - ,m_limit() -#endif { m_rbAFrame.getOrigin() = pivotInA; @@ -93,14 +93,15 @@ btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const bt btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA) -:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), m_angularOnly(false), m_enableAngularMotor(false), +:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), +#ifdef _BT_USE_CENTER_LIMIT_ +m_limit(), +#endif +m_angularOnly(false), m_enableAngularMotor(false), m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), m_useReferenceFrameA(useReferenceFrameA), m_flags(0) -#ifdef _BT_USE_CENTER_LIMIT_ -,m_limit() -#endif { // since no frame is given, assume this to be zero angle and just pick rb transform axis @@ -142,15 +143,15 @@ m_flags(0) btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btTransform& rbAFrame, const btTransform& rbBFrame, bool useReferenceFrameA) :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),m_rbAFrame(rbAFrame),m_rbBFrame(rbBFrame), +#ifdef _BT_USE_CENTER_LIMIT_ +m_limit(), +#endif m_angularOnly(false), m_enableAngularMotor(false), m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), m_useReferenceFrameA(useReferenceFrameA), m_flags(0) -#ifdef _BT_USE_CENTER_LIMIT_ -,m_limit() -#endif { #ifndef _BT_USE_CENTER_LIMIT_ //start with free @@ -168,15 +169,15 @@ m_flags(0) btHingeConstraint::btHingeConstraint(btRigidBody& rbA, const btTransform& rbAFrame, bool useReferenceFrameA) :btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),m_rbAFrame(rbAFrame),m_rbBFrame(rbAFrame), +#ifdef _BT_USE_CENTER_LIMIT_ +m_limit(), +#endif m_angularOnly(false), m_enableAngularMotor(false), m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER), m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET), m_useReferenceFrameA(useReferenceFrameA), m_flags(0) -#ifdef _BT_USE_CENTER_LIMIT_ -,m_limit() -#endif { ///not providing rigidbody B means implicitly using worldspace for body B @@ -663,7 +664,7 @@ void btHingeConstraint::setMotorTarget(const btQuaternion& qAinB, btScalar dt) btScalar targetAngle = qHinge.getAngle(); if (targetAngle > SIMD_PI) // long way around. flip quat and recalculate. { - qHinge = operator-(qHinge); + qHinge = -(qHinge); targetAngle = qHinge.getAngle(); } if (qHinge.getZ() < 0) diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h index 50e3f73cb6d..cb2973e1d1d 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h @@ -15,8 +15,9 @@ subject to the following restrictions: /* Hinge Constraint by Dirk Gregorius. Limits added by Marcus Hennix at Starbreeze Studios */ -#ifndef HINGECONSTRAINT_H -#define HINGECONSTRAINT_H +#ifndef BT_HINGECONSTRAINT_H +#define BT_HINGECONSTRAINT_H + #define _BT_USE_CENTER_LIMIT_ 1 @@ -377,4 +378,4 @@ SIMD_FORCE_INLINE const char* btHingeConstraint::serialize(void* dataBuffer, btS return btHingeConstraintDataName; } -#endif //HINGECONSTRAINT_H +#endif //BT_HINGECONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h index 22a8af66b8e..f1994a2dfd8 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef JACOBIAN_ENTRY_H -#define JACOBIAN_ENTRY_H +#ifndef BT_JACOBIAN_ENTRY_H +#define BT_JACOBIAN_ENTRY_H #include "LinearMath/btVector3.h" #include "BulletDynamics/Dynamics/btRigidBody.h" @@ -153,4 +153,4 @@ public: }; -#endif //JACOBIAN_ENTRY_H +#endif //BT_JACOBIAN_ENTRY_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h index b589ee68254..b3bda03eec1 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef POINT2POINTCONSTRAINT_H -#define POINT2POINTCONSTRAINT_H +#ifndef BT_POINT2POINTCONSTRAINT_H +#define BT_POINT2POINTCONSTRAINT_H #include "LinearMath/btVector3.h" #include "btJacobianEntry.h" @@ -158,4 +158,4 @@ SIMD_FORCE_INLINE const char* btPoint2PointConstraint::serialize(void* dataBuffe return btPoint2PointConstraintDataName; } -#endif //POINT2POINTCONSTRAINT_H +#endif //BT_POINT2POINTCONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index 0fc93cd3756..ab074224028 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -225,7 +225,7 @@ void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFri resultUpperLess = _mm_cmplt_ps(sum,upperLimit1); __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp); deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) ); - c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) ); + c.m_appliedPushImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) ); __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128); __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128); __m128 impulseMagnitude = deltaImpulse; @@ -740,13 +740,13 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m } } - btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc) { BT_PROFILE("solveGroupCacheFriendlySetup"); (void)stackAlloc; (void)debugDrawer; + m_maxOverrideNumSolverIterations = 0; if (!(numConstraints + numManifolds)) { @@ -788,6 +788,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol { btTypedConstraint* constraint = constraints[j]; constraint->buildJacobian(); + constraint->internalSetAppliedImpulse(0.0f); } } //btRigidBody* rb0=0,*rb1=0; @@ -804,7 +805,14 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol for (i=0;i<numConstraints;i++) { btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i]; - constraints[i]->getInfo1(&info1); + if (constraints[i]->isEnabled()) + { + constraints[i]->getInfo1(&info1); + } else + { + info1.m_numConstraintRows = 0; + info1.nub = 0; + } totalNumRows += info1.m_numConstraintRows; } m_tmpSolverNonContactConstraintPool.resize(totalNumRows); @@ -823,23 +831,26 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol btSolverConstraint* currentConstraintRow = &m_tmpSolverNonContactConstraintPool[currentRow]; btTypedConstraint* constraint = constraints[i]; + btRigidBody& rbA = constraint->getRigidBodyA(); + btRigidBody& rbB = constraint->getRigidBodyB(); + int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations; + if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations) + m_maxOverrideNumSolverIterations = overrideNumSolverIterations; - btRigidBody& rbA = constraint->getRigidBodyA(); - btRigidBody& rbB = constraint->getRigidBodyB(); - int j; for ( j=0;j<info1.m_numConstraintRows;j++) { memset(¤tConstraintRow[j],0,sizeof(btSolverConstraint)); - currentConstraintRow[j].m_lowerLimit = -FLT_MAX; - currentConstraintRow[j].m_upperLimit = FLT_MAX; + currentConstraintRow[j].m_lowerLimit = -SIMD_INFINITY; + currentConstraintRow[j].m_upperLimit = SIMD_INFINITY; currentConstraintRow[j].m_appliedImpulse = 0.f; currentConstraintRow[j].m_appliedPushImpulse = 0.f; currentConstraintRow[j].m_solverBodyA = &rbA; currentConstraintRow[j].m_solverBodyB = &rbB; + currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations; } rbA.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); @@ -872,6 +883,17 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol for ( j=0;j<info1.m_numConstraintRows;j++) { btSolverConstraint& solverConstraint = currentConstraintRow[j]; + + if (solverConstraint.m_upperLimit>=constraints[i]->getBreakingImpulseThreshold()) + { + solverConstraint.m_upperLimit = constraints[i]->getBreakingImpulseThreshold(); + } + + if (solverConstraint.m_lowerLimit<=-constraints[i]->getBreakingImpulseThreshold()) + { + solverConstraint.m_lowerLimit = -constraints[i]->getBreakingImpulseThreshold(); + } + solverConstraint.m_originalContactPoint = constraint; { @@ -939,15 +961,20 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol btContactSolverInfo info = infoGlobal; - + int numNonContactPool = m_tmpSolverNonContactConstraintPool.size(); int numConstraintPool = m_tmpSolverContactConstraintPool.size(); int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size(); ///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints + m_orderNonContactConstraintPool.resize(numNonContactPool); m_orderTmpConstraintPool.resize(numConstraintPool); m_orderFrictionConstraintPool.resize(numFrictionPool); { int i; + for (i=0;i<numNonContactPool;i++) + { + m_orderNonContactConstraintPool[i] = i; + } for (i=0;i<numConstraintPool;i++) { m_orderTmpConstraintPool[i] = i; @@ -965,6 +992,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/) { + int numNonContactPool = m_tmpSolverNonContactConstraintPool.size(); int numConstraintPool = m_tmpSolverContactConstraintPool.size(); int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size(); @@ -973,18 +1001,29 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER) { if ((iteration & 7) == 0) { - for (j=0; j<numConstraintPool; ++j) { - int tmp = m_orderTmpConstraintPool[j]; + for (j=0; j<numNonContactPool; ++j) { + int tmp = m_orderNonContactConstraintPool[j]; int swapi = btRandInt2(j+1); - m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi]; - m_orderTmpConstraintPool[swapi] = tmp; + m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi]; + m_orderNonContactConstraintPool[swapi] = tmp; } - for (j=0; j<numFrictionPool; ++j) { - int tmp = m_orderFrictionConstraintPool[j]; - int swapi = btRandInt2(j+1); - m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi]; - m_orderFrictionConstraintPool[swapi] = tmp; + //contact/friction constraints are not solved more than + if (iteration< infoGlobal.m_numIterations) + { + for (j=0; j<numConstraintPool; ++j) { + int tmp = m_orderTmpConstraintPool[j]; + int swapi = btRandInt2(j+1); + m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi]; + m_orderTmpConstraintPool[swapi] = tmp; + } + + for (j=0; j<numFrictionPool; ++j) { + int tmp = m_orderFrictionConstraintPool[j]; + int swapi = btRandInt2(j+1); + m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi]; + m_orderFrictionConstraintPool[swapi] = tmp; + } } } } @@ -994,36 +1033,41 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration ///solve all joint constraints, using SIMD, if available for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) { - btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j]; - resolveSingleConstraintRowGenericSIMD(*constraint.m_solverBodyA,*constraint.m_solverBodyB,constraint); + btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]]; + if (iteration < constraint.m_overrideNumSolverIterations) + resolveSingleConstraintRowGenericSIMD(*constraint.m_solverBodyA,*constraint.m_solverBodyB,constraint); } - for (j=0;j<numConstraints;j++) + if (iteration< infoGlobal.m_numIterations) { - constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); - } - - ///solve all contact constraints using SIMD, if available - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - for (j=0;j<numPoolConstraints;j++) - { - const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; - resolveSingleConstraintRowLowerLimitSIMD(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold); + for (j=0;j<numConstraints;j++) + { + constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); + } - } - ///solve all friction constraints, using SIMD, if available - int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); - for (j=0;j<numFrictionPoolConstraints;j++) - { - btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; - btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; + ///solve all contact constraints using SIMD, if available + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); + for (j=0;j<numPoolConstraints;j++) + { + const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; + resolveSingleConstraintRowLowerLimitSIMD(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold); - if (totalImpulse>btScalar(0)) + } + + ///solve all friction constraints, using SIMD, if available + int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); + for (j=0;j<numFrictionPoolConstraints;j++) { - solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); - solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; + btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; + btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; + + if (totalImpulse>btScalar(0)) + { + solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); + solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; - resolveSingleConstraintRowGenericSIMD(*solveManifold.m_solverBodyA, *solveManifold.m_solverBodyB,solveManifold); + resolveSingleConstraintRowGenericSIMD(*solveManifold.m_solverBodyA, *solveManifold.m_solverBodyB,solveManifold); + } } } } else @@ -1032,34 +1076,38 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration ///solve all joint constraints for (j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) { - btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[j]; - resolveSingleConstraintRowGeneric(*constraint.m_solverBodyA,*constraint.m_solverBodyB,constraint); + btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]]; + if (iteration < constraint.m_overrideNumSolverIterations) + resolveSingleConstraintRowGeneric(*constraint.m_solverBodyA,*constraint.m_solverBodyB,constraint); } - for (j=0;j<numConstraints;j++) - { - constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); - } - ///solve all contact constraints - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - for (j=0;j<numPoolConstraints;j++) + if (iteration< infoGlobal.m_numIterations) { - const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; - resolveSingleConstraintRowLowerLimit(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold); - } - ///solve all friction constraints - int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); - for (j=0;j<numFrictionPoolConstraints;j++) - { - btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; - btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; - - if (totalImpulse>btScalar(0)) + for (j=0;j<numConstraints;j++) { - solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); - solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; + constraints[j]->solveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); + } + ///solve all contact constraints + int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); + for (j=0;j<numPoolConstraints;j++) + { + const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; + resolveSingleConstraintRowLowerLimit(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold); + } + ///solve all friction constraints + int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); + for (j=0;j<numFrictionPoolConstraints;j++) + { + btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]]; + btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; - resolveSingleConstraintRowGeneric(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold); + if (totalImpulse>btScalar(0)) + { + solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); + solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; + + resolveSingleConstraintRowGeneric(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold); + } } } } @@ -1111,16 +1159,18 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations( { BT_PROFILE("solveGroupCacheFriendlyIterations"); - - //should traverse the contacts random order... - int iteration; { - for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++) + ///this is a special step to resolve penetrations (just for contacts) + solveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc); + + int maxIterations = m_maxOverrideNumSolverIterations > infoGlobal.m_numIterations? m_maxOverrideNumSolverIterations : infoGlobal.m_numIterations; + + for ( int iteration = 0 ; iteration< maxIterations ; iteration++) + //for ( int iteration = maxIterations-1 ; iteration >= 0;iteration--) { solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc); } - solveGroupCacheFriendlySplitImpulseIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer,stackAlloc); } return 0.f; } @@ -1151,9 +1201,11 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCo { const btSolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j]; btTypedConstraint* constr = (btTypedConstraint*)solverConstr.m_originalContactPoint; - btScalar sum = constr->internalGetAppliedImpulse(); - sum += solverConstr.m_appliedImpulse; - constr->internalSetAppliedImpulse(sum); + constr->internalSetAppliedImpulse(solverConstr.m_appliedImpulse); + if (btFabs(solverConstr.m_appliedImpulse)>=constr->getBreakingImpulseThreshold()) + { + constr->setEnabled(false); + } } diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index f268ec9ae74..bb377db8db9 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H -#define SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H +#ifndef BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H +#define BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H #include "btConstraintSolver.h" class btIDebugDraw; @@ -33,8 +33,10 @@ protected: btConstraintArray m_tmpSolverNonContactConstraintPool; btConstraintArray m_tmpSolverContactFrictionConstraintPool; btAlignedObjectArray<int> m_orderTmpConstraintPool; + btAlignedObjectArray<int> m_orderNonContactConstraintPool; btAlignedObjectArray<int> m_orderFrictionConstraintPool; btAlignedObjectArray<btTypedConstraint::btConstraintInfo1> m_tmpConstraintSizesPool; + int m_maxOverrideNumSolverIterations; void setupFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyIdB, btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2, @@ -124,5 +126,5 @@ typedef btSequentialImpulseConstraintSolver btSequentialImpulseConstraintSolverP #endif -#endif //SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H +#endif //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h index e9723a7bb40..2edc8d2b2e1 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h @@ -22,8 +22,8 @@ TODO: - add conversion for ODE constraint solver */ -#ifndef SLIDER_CONSTRAINT_H -#define SLIDER_CONSTRAINT_H +#ifndef BT_SLIDER_CONSTRAINT_H +#define BT_SLIDER_CONSTRAINT_H @@ -236,7 +236,10 @@ public: btScalar getTargetAngMotorVelocity() { return m_targetAngMotorVelocity; } void setMaxAngMotorForce(btScalar maxAngMotorForce) { m_maxAngMotorForce = maxAngMotorForce; } btScalar getMaxAngMotorForce() { return m_maxAngMotorForce; } - btScalar getLinearPos() { return m_linPos; } + + btScalar getLinearPos() const { return m_linPos; } + btScalar getAngularPos() const { return m_angPos; } + // access for ODE solver @@ -326,5 +329,5 @@ SIMD_FORCE_INLINE const char* btSliderConstraint::serialize(void* dataBuffer, bt -#endif //SLIDER_CONSTRAINT_H +#endif //BT_SLIDER_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h index 057d3fac827..e8bfabf864e 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef SOLVE_2LINEAR_CONSTRAINT_H -#define SOLVE_2LINEAR_CONSTRAINT_H +#ifndef BT_SOLVE_2LINEAR_CONSTRAINT_H +#define BT_SOLVE_2LINEAR_CONSTRAINT_H #include "LinearMath/btMatrix3x3.h" #include "LinearMath/btVector3.h" @@ -104,4 +104,4 @@ public: }; -#endif //SOLVE_2LINEAR_CONSTRAINT_H +#endif //BT_SOLVE_2LINEAR_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h index 79e45a4383b..179e79d7911 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h @@ -51,6 +51,8 @@ ATTRIBUTE_ALIGNED64 (struct) btSolverConstraint btScalar m_unusedPadding0; }; + int m_overrideNumSolverIterations; + union { int m_frictionIndex; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp index 8212c36ff4e..06bde5e7eec 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp @@ -25,7 +25,10 @@ btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rb :btTypedObject(type), m_userConstraintType(-1), m_userConstraintId(-1), +m_breakingImpulseThreshold(SIMD_INFINITY), +m_isEnabled(true), m_needsFeedback(false), +m_overrideNumSolverIterations(-1), m_rbA(rbA), m_rbB(getFixedBody()), m_appliedImpulse(btScalar(0.)), @@ -38,7 +41,10 @@ btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rb :btTypedObject(type), m_userConstraintType(-1), m_userConstraintId(-1), +m_breakingImpulseThreshold(SIMD_INFINITY), +m_isEnabled(true), m_needsFeedback(false), +m_overrideNumSolverIterations(-1), m_rbA(rbA), m_rbB(rbB), m_appliedImpulse(btScalar(0.)), @@ -114,6 +120,10 @@ const char* btTypedConstraint::serialize(void* dataBuffer, btSerializer* seriali tcd->m_objectType = m_objectType; tcd->m_needsFeedback = m_needsFeedback; + tcd->m_overrideNumSolverIterations = m_overrideNumSolverIterations; + tcd->m_breakingImpulseThreshold = float(m_breakingImpulseThreshold); + tcd->m_isEnabled = m_isEnabled? 1: 0; + tcd->m_userConstraintId =m_userConstraintId; tcd->m_userConstraintType =m_userConstraintType; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h index 65e6d558300..a16e869a973 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef TYPED_CONSTRAINT_H -#define TYPED_CONSTRAINT_H +#ifndef BT_TYPED_CONSTRAINT_H +#define BT_TYPED_CONSTRAINT_H class btRigidBody; #include "LinearMath/btScalar.h" @@ -62,7 +62,11 @@ class btTypedConstraint : public btTypedObject void* m_userConstraintPtr; }; - bool m_needsFeedback; + btScalar m_breakingImpulseThreshold; + bool m_isEnabled; + bool m_needsFeedback; + int m_overrideNumSolverIterations; + btTypedConstraint& operator=(btTypedConstraint& other) { @@ -80,7 +84,6 @@ protected: ///internal method used by the constraint solver, don't use them directly btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact); - static btRigidBody& getFixedBody(); public: @@ -92,6 +95,8 @@ public: int m_numConstraintRows,nub; }; + static btRigidBody& getFixedBody(); + struct btConstraintInfo2 { // integrator parameters: frames per second (1/stepsize), default error // reduction parameter (0..1). @@ -126,6 +131,18 @@ public: btScalar m_damping; }; + int getOverrideNumSolverIterations() const + { + return m_overrideNumSolverIterations; + } + + ///override the number of constraint solver iterations used to solve this constraint + ///-1 will use the default number of iterations, as specified in SolverInfo.m_numIterations + void setOverrideNumSolverIterations(int overideNumIterations) + { + m_overrideNumSolverIterations = overideNumIterations; + } + ///internal method used by the constraint solver, don't use them directly virtual void buildJacobian() {}; @@ -155,6 +172,28 @@ public: return m_appliedImpulse; } + + btScalar getBreakingImpulseThreshold() const + { + return m_breakingImpulseThreshold; + } + + void setBreakingImpulseThreshold(btScalar threshold) + { + m_breakingImpulseThreshold = threshold; + } + + bool isEnabled() const + { + return m_isEnabled; + } + + void setEnabled(bool enabled) + { + m_isEnabled=enabled; + } + + ///internal method used by the constraint solver, don't use them directly virtual void solveConstraintObsolete(btRigidBody& /*bodyA*/,btRigidBody& /*bodyB*/,btScalar /*timeStep*/) {}; @@ -302,7 +341,10 @@ struct btTypedConstraintData float m_dbgDrawSize; int m_disableCollisionsBetweenLinkedBodies; - char m_pad4[4]; + int m_overrideNumSolverIterations; + + float m_breakingImpulseThreshold; + int m_isEnabled; }; @@ -407,4 +449,4 @@ public: -#endif //TYPED_CONSTRAINT_H +#endif //BT_TYPED_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp index 40c56e77954..b009f41aec8 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.cpp @@ -27,7 +27,7 @@ subject to the following restrictions: // constructor // anchor, axis1 and axis2 are in world coordinate system // axis1 must be orthogonal to axis2 -btUniversalConstraint::btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2) +btUniversalConstraint::btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& anchor, const btVector3& axis1, const btVector3& axis2) : btGeneric6DofConstraint(rbA, rbB, btTransform::getIdentity(), btTransform::getIdentity(), true), m_anchor(anchor), m_axis1(axis1), @@ -42,8 +42,8 @@ btUniversalConstraint::btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, // new position of X, allowed limits are (-PI,PI); // So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs // Build the frame in world coordinate system first - btVector3 zAxis = axis1.normalize(); - btVector3 yAxis = axis2.normalize(); + btVector3 zAxis = m_axis1.normalize(); + btVector3 yAxis = m_axis2.normalize(); btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system btTransform frameInW; frameInW.setIdentity(); diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h index a8d404a591c..a86939164ec 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef UNIVERSAL_CONSTRAINT_H -#define UNIVERSAL_CONSTRAINT_H +#ifndef BT_UNIVERSAL_CONSTRAINT_H +#define BT_UNIVERSAL_CONSTRAINT_H @@ -41,7 +41,7 @@ public: // constructor // anchor, axis1 and axis2 are in world coordinate system // axis1 must be orthogonal to axis2 - btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, btVector3& anchor, btVector3& axis1, btVector3& axis2); + btUniversalConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& anchor, const btVector3& axis1, const btVector3& axis2); // access const btVector3& getAnchor() { return m_calculatedTransformA.getOrigin(); } const btVector3& getAnchor2() { return m_calculatedTransformB.getOrigin(); } @@ -58,5 +58,5 @@ public: -#endif // UNIVERSAL_CONSTRAINT_H +#endif // BT_UNIVERSAL_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp deleted file mode 100644 index 23501c4435e..00000000000 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.cpp +++ /dev/null @@ -1,196 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - -#include "btContinuousDynamicsWorld.h" -#include "LinearMath/btQuickprof.h" - -//collision detection -#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h" -#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h" -#include "BulletCollision/CollisionShapes/btCollisionShape.h" -#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" - -//rigidbody & constraints -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" -#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" -#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" - - - -#include <stdio.h> - -btContinuousDynamicsWorld::btContinuousDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration) -:btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration) -{ -} - -btContinuousDynamicsWorld::~btContinuousDynamicsWorld() -{ -} - - -void btContinuousDynamicsWorld::internalSingleStepSimulation( btScalar timeStep) -{ - - startProfiling(timeStep); - - if(0 != m_internalPreTickCallback) { - (*m_internalPreTickCallback)(this, timeStep); - } - - - ///update aabbs information - updateAabbs(); - //static int frame=0; -// printf("frame %d\n",frame++); - - ///apply gravity, predict motion - predictUnconstraintMotion(timeStep); - - btDispatcherInfo& dispatchInfo = getDispatchInfo(); - - dispatchInfo.m_timeStep = timeStep; - dispatchInfo.m_stepCount = 0; - dispatchInfo.m_debugDraw = getDebugDrawer(); - - ///perform collision detection - performDiscreteCollisionDetection(); - - calculateSimulationIslands(); - - - getSolverInfo().m_timeStep = timeStep; - - - - ///solve contact and other joint constraints - solveConstraints(getSolverInfo()); - - ///CallbackTriggers(); - calculateTimeOfImpacts(timeStep); - - btScalar toi = dispatchInfo.m_timeOfImpact; -// if (toi < 1.f) -// printf("toi = %f\n",toi); - if (toi < 0.f) - printf("toi = %f\n",toi); - - - ///integrate transforms - integrateTransforms(timeStep * toi); - - ///update vehicle simulation - updateActions(timeStep); - - updateActivationState( timeStep ); - - if(0 != m_internalTickCallback) { - (*m_internalTickCallback)(this, timeStep); - } -} - -void btContinuousDynamicsWorld::calculateTimeOfImpacts(btScalar timeStep) -{ - ///these should be 'temporal' aabbs! - updateTemporalAabbs(timeStep); - - ///'toi' is the global smallest time of impact. However, we just calculate the time of impact for each object individually. - ///so we handle the case moving versus static properly, and we cheat for moving versus moving - btScalar toi = 1.f; - - - btDispatcherInfo& dispatchInfo = getDispatchInfo(); - dispatchInfo.m_timeStep = timeStep; - dispatchInfo.m_timeOfImpact = 1.f; - dispatchInfo.m_stepCount = 0; - dispatchInfo.m_dispatchFunc = btDispatcherInfo::DISPATCH_CONTINUOUS; - - ///calculate time of impact for overlapping pairs - - - btDispatcher* dispatcher = getDispatcher(); - if (dispatcher) - dispatcher->dispatchAllCollisionPairs(m_broadphasePairCache->getOverlappingPairCache(),dispatchInfo,m_dispatcher1); - - toi = dispatchInfo.m_timeOfImpact; - - dispatchInfo.m_dispatchFunc = btDispatcherInfo::DISPATCH_DISCRETE; - -} - -void btContinuousDynamicsWorld::updateTemporalAabbs(btScalar timeStep) -{ - - btVector3 temporalAabbMin,temporalAabbMax; - - for ( int i=0;i<m_collisionObjects.size();i++) - { - btCollisionObject* colObj = m_collisionObjects[i]; - - btRigidBody* body = btRigidBody::upcast(colObj); - if (body) - { - body->getCollisionShape()->getAabb(m_collisionObjects[i]->getWorldTransform(),temporalAabbMin,temporalAabbMax); - const btVector3& linvel = body->getLinearVelocity(); - - //make the AABB temporal - btScalar temporalAabbMaxx = temporalAabbMax.getX(); - btScalar temporalAabbMaxy = temporalAabbMax.getY(); - btScalar temporalAabbMaxz = temporalAabbMax.getZ(); - btScalar temporalAabbMinx = temporalAabbMin.getX(); - btScalar temporalAabbMiny = temporalAabbMin.getY(); - btScalar temporalAabbMinz = temporalAabbMin.getZ(); - - // add linear motion - btVector3 linMotion = linvel*timeStep; - - if (linMotion.x() > 0.f) - temporalAabbMaxx += linMotion.x(); - else - temporalAabbMinx += linMotion.x(); - if (linMotion.y() > 0.f) - temporalAabbMaxy += linMotion.y(); - else - temporalAabbMiny += linMotion.y(); - if (linMotion.z() > 0.f) - temporalAabbMaxz += linMotion.z(); - else - temporalAabbMinz += linMotion.z(); - - //add conservative angular motion - btScalar angularMotion(0);// = angvel.length() * GetAngularMotionDisc() * timeStep; - btVector3 angularMotion3d(angularMotion,angularMotion,angularMotion); - temporalAabbMin = btVector3(temporalAabbMinx,temporalAabbMiny,temporalAabbMinz); - temporalAabbMax = btVector3(temporalAabbMaxx,temporalAabbMaxy,temporalAabbMaxz); - - temporalAabbMin -= angularMotion3d; - temporalAabbMax += angularMotion3d; - - m_broadphasePairCache->setAabb(body->getBroadphaseHandle(),temporalAabbMin,temporalAabbMax,m_dispatcher1); - } - } - - //update aabb (of all moved objects) - - m_broadphasePairCache->calculateOverlappingPairs(m_dispatcher1); - - - -} - - - diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.h deleted file mode 100644 index 61c8dea03eb..00000000000 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btContinuousDynamicsWorld.h +++ /dev/null @@ -1,46 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2007 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BT_CONTINUOUS_DYNAMICS_WORLD_H -#define BT_CONTINUOUS_DYNAMICS_WORLD_H - -#include "btDiscreteDynamicsWorld.h" - -///btContinuousDynamicsWorld adds optional (per object) continuous collision detection for fast moving objects to the btDiscreteDynamicsWorld. -///This copes with fast moving objects that otherwise would tunnel/miss collisions. -///Under construction, don't use yet! Please use btDiscreteDynamicsWorld instead. -class btContinuousDynamicsWorld : public btDiscreteDynamicsWorld -{ - - void updateTemporalAabbs(btScalar timeStep); - - public: - - btContinuousDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration); - virtual ~btContinuousDynamicsWorld(); - - ///time stepping with calculation of time of impact for selected fast moving objects - virtual void internalSingleStepSimulation( btScalar timeStep); - - virtual void calculateTimeOfImpacts(btScalar timeStep); - - virtual btDynamicsWorldType getWorldType() const - { - return BT_CONTINUOUS_DYNAMICS_WORLD; - } - -}; - -#endif //BT_CONTINUOUS_DYNAMICS_WORLD_H diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp index e1eada07c82..954ef241adc 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @@ -35,6 +35,8 @@ subject to the following restrictions: #include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h" #include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h" #include "BulletDynamics/ConstraintSolver/btSliderConstraint.h" +#include "BulletDynamics/ConstraintSolver/btContactConstraint.h" + #include "LinearMath/btIDebugDraw.h" #include "BulletCollision/CollisionShapes/btSphereShape.h" @@ -46,6 +48,167 @@ subject to the following restrictions: #include "LinearMath/btSerializer.h" +#if 0 +btAlignedObjectArray<btVector3> debugContacts; +btAlignedObjectArray<btVector3> debugNormals; +int startHit=2; +int firstHit=startHit; +#endif + +SIMD_FORCE_INLINE int btGetConstraintIslandId(const btTypedConstraint* lhs) +{ + int islandId; + + const btCollisionObject& rcolObj0 = lhs->getRigidBodyA(); + const btCollisionObject& rcolObj1 = lhs->getRigidBodyB(); + islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag(); + return islandId; + +} + + +class btSortConstraintOnIslandPredicate +{ + public: + + bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs ) const + { + int rIslandId0,lIslandId0; + rIslandId0 = btGetConstraintIslandId(rhs); + lIslandId0 = btGetConstraintIslandId(lhs); + return lIslandId0 < rIslandId0; + } +}; + +struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback +{ + btContactSolverInfo* m_solverInfo; + btConstraintSolver* m_solver; + btTypedConstraint** m_sortedConstraints; + int m_numConstraints; + btIDebugDraw* m_debugDrawer; + btStackAlloc* m_stackAlloc; + btDispatcher* m_dispatcher; + + btAlignedObjectArray<btCollisionObject*> m_bodies; + btAlignedObjectArray<btPersistentManifold*> m_manifolds; + btAlignedObjectArray<btTypedConstraint*> m_constraints; + + + InplaceSolverIslandCallback( + btConstraintSolver* solver, + btStackAlloc* stackAlloc, + btDispatcher* dispatcher) + :m_solverInfo(NULL), + m_solver(solver), + m_sortedConstraints(NULL), + m_numConstraints(0), + m_debugDrawer(NULL), + m_stackAlloc(stackAlloc), + m_dispatcher(dispatcher) + { + + } + + InplaceSolverIslandCallback& operator=(InplaceSolverIslandCallback& other) + { + btAssert(0); + (void)other; + return *this; + } + + SIMD_FORCE_INLINE void setup ( btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btIDebugDraw* debugDrawer) + { + btAssert(solverInfo); + m_solverInfo = solverInfo; + m_sortedConstraints = sortedConstraints; + m_numConstraints = numConstraints; + m_debugDrawer = debugDrawer; + m_bodies.resize (0); + m_manifolds.resize (0); + m_constraints.resize (0); + } + + + virtual void processIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifolds,int numManifolds, int islandId) + { + if (islandId<0) + { + if (numManifolds + m_numConstraints) + { + ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id + m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,*m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher); + } + } else + { + //also add all non-contact constraints/joints for this island + btTypedConstraint** startConstraint = 0; + int numCurConstraints = 0; + int i; + + //find the first constraint for this island + for (i=0;i<m_numConstraints;i++) + { + if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId) + { + startConstraint = &m_sortedConstraints[i]; + break; + } + } + //count the number of constraints in this island + for (;i<m_numConstraints;i++) + { + if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId) + { + numCurConstraints++; + } + } + + if (m_solverInfo->m_minimumSolverBatchSize<=1) + { + ///only call solveGroup if there is some work: avoid virtual function call, its overhead can be excessive + if (numManifolds + numCurConstraints) + { + m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher); + } + } else + { + + for (i=0;i<numBodies;i++) + m_bodies.push_back(bodies[i]); + for (i=0;i<numManifolds;i++) + m_manifolds.push_back(manifolds[i]); + for (i=0;i<numCurConstraints;i++) + m_constraints.push_back(startConstraint[i]); + if ((m_constraints.size()+m_manifolds.size())>m_solverInfo->m_minimumSolverBatchSize) + { + processConstraints(); + } else + { + //printf("deferred\n"); + } + } + } + } + void processConstraints() + { + if (m_manifolds.size() + m_constraints.size()>0) + { + + btCollisionObject** bodies = m_bodies.size()? &m_bodies[0]:0; + btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0; + btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0; + + m_solver->solveGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,*m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher); + } + m_bodies.resize(0); + m_manifolds.resize(0); + m_constraints.resize(0); + + } + +}; + btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration) @@ -54,7 +217,9 @@ m_constraintSolver(constraintSolver), m_gravity(0,-10,0), m_localTime(0), m_synchronizeAllMotionStates(false), -m_profileTimings(0) +m_profileTimings(0), +m_sortedConstraints (), +m_solverIslandCallback ( NULL ) { if (!m_constraintSolver) { @@ -72,6 +237,11 @@ m_profileTimings(0) } m_ownsIslandManager = true; + + { + void* mem = btAlignedAlloc(sizeof(InplaceSolverIslandCallback),16); + m_solverIslandCallback = new (mem) InplaceSolverIslandCallback (constraintSolver, m_stackAlloc, dispatcher); + } } @@ -83,6 +253,11 @@ btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld() m_islandManager->~btSimulationIslandManager(); btAlignedFree( m_islandManager); } + if (m_solverIslandCallback) + { + m_solverIslandCallback->~InplaceSolverIslandCallback(); + btAlignedFree(m_solverIslandCallback); + } if (m_ownsConstraintSolver) { @@ -138,7 +313,7 @@ void btDiscreteDynamicsWorld::debugDrawWorld() - if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb)) + if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb | btIDebugDraw::DBG_DrawNormals))) { int i; @@ -314,9 +489,11 @@ void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) dispatchInfo.m_stepCount = 0; dispatchInfo.m_debugDraw = getDebugDrawer(); + ///perform collision detection performDiscreteCollisionDetection(); + calculateSimulationIslands(); @@ -524,187 +701,39 @@ void btDiscreteDynamicsWorld::removeCharacter(btActionInterface* character) } -SIMD_FORCE_INLINE int btGetConstraintIslandId(const btTypedConstraint* lhs) -{ - int islandId; - - const btCollisionObject& rcolObj0 = lhs->getRigidBodyA(); - const btCollisionObject& rcolObj1 = lhs->getRigidBodyB(); - islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag(); - return islandId; - -} - - -class btSortConstraintOnIslandPredicate -{ - public: - - bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs ) - { - int rIslandId0,lIslandId0; - rIslandId0 = btGetConstraintIslandId(rhs); - lIslandId0 = btGetConstraintIslandId(lhs); - return lIslandId0 < rIslandId0; - } -}; - void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) { BT_PROFILE("solveConstraints"); - struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback - { - - btContactSolverInfo& m_solverInfo; - btConstraintSolver* m_solver; - btTypedConstraint** m_sortedConstraints; - int m_numConstraints; - btIDebugDraw* m_debugDrawer; - btStackAlloc* m_stackAlloc; - btDispatcher* m_dispatcher; - - btAlignedObjectArray<btCollisionObject*> m_bodies; - btAlignedObjectArray<btPersistentManifold*> m_manifolds; - btAlignedObjectArray<btTypedConstraint*> m_constraints; - - - InplaceSolverIslandCallback( - btContactSolverInfo& solverInfo, - btConstraintSolver* solver, - btTypedConstraint** sortedConstraints, - int numConstraints, - btIDebugDraw* debugDrawer, - btStackAlloc* stackAlloc, - btDispatcher* dispatcher) - :m_solverInfo(solverInfo), - m_solver(solver), - m_sortedConstraints(sortedConstraints), - m_numConstraints(numConstraints), - m_debugDrawer(debugDrawer), - m_stackAlloc(stackAlloc), - m_dispatcher(dispatcher) - { - - } - - - InplaceSolverIslandCallback& operator=(InplaceSolverIslandCallback& other) - { - btAssert(0); - (void)other; - return *this; - } - virtual void ProcessIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifolds,int numManifolds, int islandId) - { - if (islandId<0) - { - if (numManifolds + m_numConstraints) - { - ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id - m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher); - } - } else - { - //also add all non-contact constraints/joints for this island - btTypedConstraint** startConstraint = 0; - int numCurConstraints = 0; - int i; - - //find the first constraint for this island - for (i=0;i<m_numConstraints;i++) - { - if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId) - { - startConstraint = &m_sortedConstraints[i]; - break; - } - } - //count the number of constraints in this island - for (;i<m_numConstraints;i++) - { - if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId) - { - numCurConstraints++; - } - } - - if (m_solverInfo.m_minimumSolverBatchSize<=1) - { - ///only call solveGroup if there is some work: avoid virtual function call, its overhead can be excessive - if (numManifolds + numCurConstraints) - { - m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher); - } - } else - { - - for (i=0;i<numBodies;i++) - m_bodies.push_back(bodies[i]); - for (i=0;i<numManifolds;i++) - m_manifolds.push_back(manifolds[i]); - for (i=0;i<numCurConstraints;i++) - m_constraints.push_back(startConstraint[i]); - if ((m_constraints.size()+m_manifolds.size())>m_solverInfo.m_minimumSolverBatchSize) - { - processConstraints(); - } else - { - //printf("deferred\n"); - } - } - } - } - void processConstraints() - { - if (m_manifolds.size() + m_constraints.size()>0) - { - m_solver->solveGroup( &m_bodies[0],m_bodies.size(), &m_manifolds[0], m_manifolds.size(), &m_constraints[0], m_constraints.size() ,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher); - } - m_bodies.resize(0); - m_manifolds.resize(0); - m_constraints.resize(0); - - } - - }; - - - - //sorted version of all btTypedConstraint, based on islandId - btAlignedObjectArray<btTypedConstraint*> sortedConstraints; - sortedConstraints.resize( m_constraints.size()); + m_sortedConstraints.resize( m_constraints.size()); int i; for (i=0;i<getNumConstraints();i++) { - sortedConstraints[i] = m_constraints[i]; + m_sortedConstraints[i] = m_constraints[i]; } // btAssert(0); - sortedConstraints.quickSort(btSortConstraintOnIslandPredicate()); + m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate()); - btTypedConstraint** constraintsPtr = getNumConstraints() ? &sortedConstraints[0] : 0; - - InplaceSolverIslandCallback solverCallback( solverInfo, m_constraintSolver, constraintsPtr,sortedConstraints.size(), m_debugDrawer,m_stackAlloc,m_dispatcher1); + btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0; + m_solverIslandCallback->setup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),getDebugDrawer()); m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds()); /// solve all the constraints for this island - m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),&solverCallback); + m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverIslandCallback); - solverCallback.processConstraints(); + m_solverIslandCallback->processConstraints(); m_constraintSolver->allSolved(solverInfo, m_debugDrawer, m_stackAlloc); } - - void btDiscreteDynamicsWorld::calculateSimulationIslands() { BT_PROFILE("calculateSimulationIslands"); @@ -717,18 +746,20 @@ void btDiscreteDynamicsWorld::calculateSimulationIslands() for (i=0;i< numConstraints ; i++ ) { btTypedConstraint* constraint = m_constraints[i]; - - const btRigidBody* colObj0 = &constraint->getRigidBodyA(); - const btRigidBody* colObj1 = &constraint->getRigidBodyB(); - - if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) && - ((colObj1) && (!(colObj1)->isStaticOrKinematicObject()))) + if (constraint->isEnabled()) { - if (colObj0->isActive() || colObj1->isActive()) + const btRigidBody* colObj0 = &constraint->getRigidBodyA(); + const btRigidBody* colObj1 = &constraint->getRigidBodyB(); + + if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) && + ((colObj1) && (!(colObj1)->isStaticOrKinematicObject()))) { + if (colObj0->isActive() || colObj1->isActive()) + { - getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(), - (colObj1)->getIslandTag()); + getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(), + (colObj1)->getIslandTag()); + } } } } @@ -745,12 +776,13 @@ void btDiscreteDynamicsWorld::calculateSimulationIslands() class btClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback { +public: + btCollisionObject* m_me; btScalar m_allowedPenetration; btOverlappingPairCache* m_pairCache; btDispatcher* m_dispatcher; - public: btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : btCollisionWorld::ClosestConvexResultCallback(fromA,toA), @@ -797,6 +829,7 @@ public: //call needsResponse, see http://code.google.com/p/bullet/issues/detail?id=179 if (m_dispatcher->needsResponse(m_me,otherObj)) { +#if 0 ///don't do CCD when there are already contact points (touching contact/penetration) btAlignedObjectArray<btPersistentManifold*> manifoldArray; btBroadphasePair* collisionPair = m_pairCache->findPair(m_me->getBroadphaseHandle(),proxy0); @@ -814,8 +847,11 @@ public: } } } +#endif + return true; } - return true; + + return false; } @@ -824,7 +860,6 @@ public: ///internal debugging variable. this value shouldn't be too high int gNumClampedCcdMotions=0; -//#include "stdio.h" void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) { BT_PROFILE("integrateTransforms"); @@ -836,34 +871,93 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) if (body->isActive() && (!body->isStaticOrKinematicObject())) { + body->predictIntegratedTransform(timeStep, predictedTrans); + btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2(); - if (body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion) + + + if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion) { BT_PROFILE("CCD motion clamping"); if (body->getCollisionShape()->isConvex()) { gNumClampedCcdMotions++; - +#ifdef USE_STATIC_ONLY + class StaticOnlyCallback : public btClosestNotMeConvexResultCallback + { + public: + + StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : + btClosestNotMeConvexResultCallback(me,fromA,toA,pairCache,dispatcher) + { + } + + virtual bool needsCollision(btBroadphaseProxy* proxy0) const + { + btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject; + if (!otherObj->isStaticOrKinematicObject()) + return false; + return btClosestNotMeConvexResultCallback::needsCollision(proxy0); + } + }; + + StaticOnlyCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher()); +#else btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher()); +#endif //btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape()); btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape()); + sweepResults.m_allowedPenetration=getDispatchInfo().m_allowedCcdPenetration; sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup; sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask; + btTransform modifiedPredictedTrans = predictedTrans; + modifiedPredictedTrans.setBasis(body->getWorldTransform().getBasis()); - convexSweepTest(&tmpSphere,body->getWorldTransform(),predictedTrans,sweepResults); + convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults); if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f)) { + + //printf("clamped integration to hit fraction = %f\n",fraction); body->setHitFraction(sweepResults.m_closestHitFraction); body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans); body->setHitFraction(0.f); -// printf("clamped integration to hit fraction = %f\n",fraction); + body->proceedToTransform( predictedTrans); + +#if 0 + btVector3 linVel = body->getLinearVelocity(); + + btScalar maxSpeed = body->getCcdMotionThreshold()/getSolverInfo().m_timeStep; + btScalar maxSpeedSqr = maxSpeed*maxSpeed; + if (linVel.length2()>maxSpeedSqr) + { + linVel.normalize(); + linVel*= maxSpeed; + body->setLinearVelocity(linVel); + btScalar ms2 = body->getLinearVelocity().length2(); + body->predictIntegratedTransform(timeStep, predictedTrans); + + btScalar sm2 = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2(); + btScalar smt = body->getCcdSquareMotionThreshold(); + printf("sm2=%f\n",sm2); + } +#else + //response between two dynamic objects without friction, assuming 0 penetration depth + btScalar appliedImpulse = 0.f; + btScalar depth = 0.f; + appliedImpulse = resolveSingleCollision(body,sweepResults.m_hitCollisionObject,sweepResults.m_hitPointWorld,sweepResults.m_hitNormalWorld,getSolverInfo(), depth); + + +#endif + + continue; } } } + body->proceedToTransform( predictedTrans); } } @@ -873,6 +967,7 @@ void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep) + void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep) { BT_PROFILE("predictUnconstraintMotion"); diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h index df47c29044f..23a38dd2a12 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h @@ -27,6 +27,8 @@ class btTypedConstraint; class btActionInterface; class btIDebugDraw; +struct InplaceSolverIslandCallback; + #include "LinearMath/btAlignedObjectArray.h" @@ -35,6 +37,9 @@ class btIDebugDraw; class btDiscreteDynamicsWorld : public btDynamicsWorld { protected: + + btAlignedObjectArray<btTypedConstraint*> m_sortedConstraints; + InplaceSolverIslandCallback* m_solverIslandCallback; btConstraintSolver* m_constraintSolver; diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h index 105317920ae..6b0093371b8 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btDynamicsWorld.h @@ -32,7 +32,8 @@ enum btDynamicsWorldType { BT_SIMPLE_DYNAMICS_WORLD=1, BT_DISCRETE_DYNAMICS_WORLD=2, - BT_CONTINUOUS_DYNAMICS_WORLD=3 + BT_CONTINUOUS_DYNAMICS_WORLD=3, + BT_SOFT_RIGID_DYNAMICS_WORLD=4 }; ///The btDynamicsWorld is the interface class for several dynamics implementation, basic, discrete, parallel, and continuous etc. diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp index aefb26a1be2..911b5072394 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp @@ -286,7 +286,7 @@ btQuaternion btRigidBody::getOrientation() const void btRigidBody::setCenterOfMassTransform(const btTransform& xform) { - if (isStaticOrKinematicObject()) + if (isKinematicObject()) { m_interpolationWorldTransform = m_worldTransform; } else @@ -309,8 +309,9 @@ bool btRigidBody::checkCollideWithOverride(btCollisionObject* co) for (int i = 0; i < m_constraintRefs.size(); ++i) { btTypedConstraint* c = m_constraintRefs[i]; - if (&c->getRigidBodyA() == otherRb || &c->getRigidBodyB() == otherRb) - return false; + if (c->isEnabled()) + if (&c->getRigidBodyA() == otherRb || &c->getRigidBodyB() == otherRb) + return false; } return true; diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h index 5a7ba97ccbc..7c121e6df13 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef RIGIDBODY_H -#define RIGIDBODY_H +#ifndef BT_RIGIDBODY_H +#define BT_RIGIDBODY_H #include "LinearMath/btAlignedObjectArray.h" #include "LinearMath/btTransform.h" @@ -687,5 +687,5 @@ struct btRigidBodyDoubleData -#endif +#endif //BT_RIGIDBODY_H diff --git a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h index 07a727e2efe..d48d2e39c4d 100644 --- a/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h +++ b/extern/bullet2/src/BulletDynamics/Dynamics/btSimpleDynamicsWorld.h @@ -23,7 +23,7 @@ class btOverlappingPairCache; class btConstraintSolver; ///The btSimpleDynamicsWorld serves as unit-test and to verify more complicated and optimized dynamics worlds. -///Please use btDiscreteDynamicsWorld instead (or btContinuousDynamicsWorld once it is finished). +///Please use btDiscreteDynamicsWorld instead class btSimpleDynamicsWorld : public btDynamicsWorld { protected: diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h index ac42f1313be..f59555f94d2 100644 --- a/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h +++ b/extern/bullet2/src/BulletDynamics/Vehicle/btRaycastVehicle.h @@ -8,8 +8,8 @@ * of this software for any purpose. * It is provided "as is" without express or implied warranty. */ -#ifndef RAYCASTVEHICLE_H -#define RAYCASTVEHICLE_H +#ifndef BT_RAYCASTVEHICLE_H +#define BT_RAYCASTVEHICLE_H #include "BulletDynamics/Dynamics/btRigidBody.h" #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" @@ -232,5 +232,5 @@ public: }; -#endif //RAYCASTVEHICLE_H +#endif //BT_RAYCASTVEHICLE_H diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btVehicleRaycaster.h b/extern/bullet2/src/BulletDynamics/Vehicle/btVehicleRaycaster.h index 5112ce6d420..3cc909c6530 100644 --- a/extern/bullet2/src/BulletDynamics/Vehicle/btVehicleRaycaster.h +++ b/extern/bullet2/src/BulletDynamics/Vehicle/btVehicleRaycaster.h @@ -1,5 +1,5 @@ /* - * Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/ + * Copyright (c) 2005 Erwin Coumans http://bulletphysics.org * * Permission to use, copy, modify, distribute and sell this software * and its documentation for any purpose is hereby granted without fee, @@ -8,8 +8,8 @@ * of this software for any purpose. * It is provided "as is" without express or implied warranty. */ -#ifndef VEHICLE_RAYCASTER_H -#define VEHICLE_RAYCASTER_H +#ifndef BT_VEHICLE_RAYCASTER_H +#define BT_VEHICLE_RAYCASTER_H #include "LinearMath/btVector3.h" @@ -31,5 +31,5 @@ virtual ~btVehicleRaycaster() }; -#endif //VEHICLE_RAYCASTER_H +#endif //BT_VEHICLE_RAYCASTER_H diff --git a/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.h b/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.h index b74f8c80acb..f916053ecac 100644 --- a/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.h +++ b/extern/bullet2/src/BulletDynamics/Vehicle/btWheelInfo.h @@ -8,8 +8,8 @@ * of this software for any purpose. * It is provided "as is" without express or implied warranty. */ -#ifndef WHEEL_INFO_H -#define WHEEL_INFO_H +#ifndef BT_WHEEL_INFO_H +#define BT_WHEEL_INFO_H #include "LinearMath/btVector3.h" #include "LinearMath/btTransform.h" @@ -115,5 +115,5 @@ struct btWheelInfo }; -#endif //WHEEL_INFO_H +#endif //BT_WHEEL_INFO_H diff --git a/extern/bullet2/src/BulletSoftBody/btDefaultSoftBodySolver.cpp b/extern/bullet2/src/BulletSoftBody/btDefaultSoftBodySolver.cpp index c876ebf1fd6..d1435b65cda 100644 --- a/extern/bullet2/src/BulletSoftBody/btDefaultSoftBodySolver.cpp +++ b/extern/bullet2/src/BulletSoftBody/btDefaultSoftBodySolver.cpp @@ -35,7 +35,7 @@ btDefaultSoftBodySolver::~btDefaultSoftBodySolver() } // In this case the data is already in the soft bodies so there is no need for us to do anything -void btDefaultSoftBodySolver::copyBackToSoftBodies() +void btDefaultSoftBodySolver::copyBackToSoftBodies(bool bMove) { } diff --git a/extern/bullet2/src/BulletSoftBody/btDefaultSoftBodySolver.h b/extern/bullet2/src/BulletSoftBody/btDefaultSoftBodySolver.h index 8e7db3daf98..7d9092ce5b9 100644 --- a/extern/bullet2/src/BulletSoftBody/btDefaultSoftBodySolver.h +++ b/extern/bullet2/src/BulletSoftBody/btDefaultSoftBodySolver.h @@ -46,7 +46,7 @@ public: virtual void optimize( btAlignedObjectArray< btSoftBody * > &softBodies,bool forceUpdate=false ); - virtual void copyBackToSoftBodies(); + virtual void copyBackToSoftBodies(bool bMove = true); virtual void solveConstraints( float solverdt ); diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp b/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp index 0d19fd193e7..9c06841801c 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp +++ b/extern/bullet2/src/BulletSoftBody/btSoftBody.cpp @@ -19,9 +19,10 @@ subject to the following restrictions: #include "btSoftBodyData.h" #include "LinearMath/btSerializer.h" + // btSoftBody::btSoftBody(btSoftBodyWorldInfo* worldInfo,int node_count, const btVector3* x, const btScalar* m) -:m_worldInfo(worldInfo),m_softBodySolver(0) +:m_softBodySolver(0),m_worldInfo(worldInfo) { /* Init */ initDefaults(); @@ -357,14 +358,14 @@ void btSoftBody::appendTetra(int node0, // -void btSoftBody::appendAnchor(int node,btRigidBody* body, bool disableCollisionBetweenLinkedBodies) +void btSoftBody::appendAnchor(int node,btRigidBody* body, bool disableCollisionBetweenLinkedBodies,btScalar influence) { btVector3 local = body->getWorldTransform().inverse()*m_nodes[node].m_x; - appendAnchor(node,body,local,disableCollisionBetweenLinkedBodies); + appendAnchor(node,body,local,disableCollisionBetweenLinkedBodies,influence); } // -void btSoftBody::appendAnchor(int node,btRigidBody* body, const btVector3& localPivot,bool disableCollisionBetweenLinkedBodies) +void btSoftBody::appendAnchor(int node,btRigidBody* body, const btVector3& localPivot,bool disableCollisionBetweenLinkedBodies,btScalar influence) { if (disableCollisionBetweenLinkedBodies) { @@ -379,6 +380,7 @@ void btSoftBody::appendAnchor(int node,btRigidBody* body, const btVector3& loc a.m_body = body; a.m_local = localPivot; a.m_node->m_battach = 1; + a.m_influence = influence; m_anchors.push_back(a); } @@ -451,6 +453,167 @@ void btSoftBody::addForce(const btVector3& force,int node) } } +void btSoftBody::addAeroForceToNode(const btVector3& windVelocity,int nodeIndex) +{ + btAssert(nodeIndex >= 0 && nodeIndex < m_nodes.size()); + + const btScalar dt = m_sst.sdt; + const btScalar kLF = m_cfg.kLF; + const btScalar kDG = m_cfg.kDG; + const btScalar kPR = m_cfg.kPR; + const btScalar kVC = m_cfg.kVC; + const bool as_lift = kLF>0; + const bool as_drag = kDG>0; + const bool as_aero = as_lift || as_drag; + const bool as_vaero = as_aero && (m_cfg.aeromodel < btSoftBody::eAeroModel::F_TwoSided); + + Node& n = m_nodes[nodeIndex]; + + if( n.m_im>0 ) + { + btSoftBody::sMedium medium; + + EvaluateMedium(m_worldInfo, n.m_x, medium); + medium.m_velocity = windVelocity; + medium.m_density = m_worldInfo->air_density; + + /* Aerodynamics */ + if(as_vaero) + { + const btVector3 rel_v = n.m_v - medium.m_velocity; + const btScalar rel_v_len = rel_v.length(); + const btScalar rel_v2 = rel_v.length2(); + + if(rel_v2>SIMD_EPSILON) + { + const btVector3 rel_v_nrm = rel_v.normalized(); + btVector3 nrm = n.m_n; + + if (m_cfg.aeromodel == btSoftBody::eAeroModel::V_TwoSidedLiftDrag) + { + nrm *= (btScalar)( (btDot(nrm,rel_v) < 0) ? -1 : +1); + btVector3 fDrag(0, 0, 0); + btVector3 fLift(0, 0, 0); + + btScalar n_dot_v = nrm.dot(rel_v_nrm); + btScalar tri_area = 0.5f * n.m_area; + + fDrag = 0.5f * kDG * medium.m_density * rel_v2 * tri_area * n_dot_v * (-rel_v_nrm); + + // Check angle of attack + // cos(10º) = 0.98480 + if ( 0 < n_dot_v && n_dot_v < 0.98480f) + fLift = 0.5f * kLF * medium.m_density * rel_v_len * tri_area * btSqrt(1.0f-n_dot_v*n_dot_v) * (nrm.cross(rel_v_nrm).cross(rel_v_nrm)); + + n.m_f += fDrag; + n.m_f += fLift; + } + else if (m_cfg.aeromodel == btSoftBody::eAeroModel::V_Point || m_cfg.aeromodel == btSoftBody::eAeroModel::V_OneSided || m_cfg.aeromodel == btSoftBody::eAeroModel::V_TwoSided) + { + if (btSoftBody::eAeroModel::V_TwoSided) + nrm *= (btScalar)( (btDot(nrm,rel_v) < 0) ? -1 : +1); + + const btScalar dvn = btDot(rel_v,nrm); + /* Compute forces */ + if(dvn>0) + { + btVector3 force(0,0,0); + const btScalar c0 = n.m_area * dvn * rel_v2/2; + const btScalar c1 = c0 * medium.m_density; + force += nrm*(-c1*kLF); + force += rel_v.normalized() * (-c1 * kDG); + ApplyClampedForce(n, force, dt); + } + } + } + } + } +} + +void btSoftBody::addAeroForceToFace(const btVector3& windVelocity,int faceIndex) +{ + const btScalar dt = m_sst.sdt; + const btScalar kLF = m_cfg.kLF; + const btScalar kDG = m_cfg.kDG; + const btScalar kPR = m_cfg.kPR; + const btScalar kVC = m_cfg.kVC; + const bool as_lift = kLF>0; + const bool as_drag = kDG>0; + const bool as_aero = as_lift || as_drag; + const bool as_faero = as_aero && (m_cfg.aeromodel >= btSoftBody::eAeroModel::F_TwoSided); + + if(as_faero) + { + btSoftBody::Face& f=m_faces[faceIndex]; + + btSoftBody::sMedium medium; + + const btVector3 v=(f.m_n[0]->m_v+f.m_n[1]->m_v+f.m_n[2]->m_v)/3; + const btVector3 x=(f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3; + EvaluateMedium(m_worldInfo,x,medium); + medium.m_velocity = windVelocity; + medium.m_density = m_worldInfo->air_density; + const btVector3 rel_v=v-medium.m_velocity; + const btScalar rel_v_len = rel_v.length(); + const btScalar rel_v2=rel_v.length2(); + + if(rel_v2>SIMD_EPSILON) + { + const btVector3 rel_v_nrm = rel_v.normalized(); + btVector3 nrm = f.m_normal; + + if (m_cfg.aeromodel == btSoftBody::eAeroModel::F_TwoSidedLiftDrag) + { + nrm *= (btScalar)( (btDot(nrm,rel_v) < 0) ? -1 : +1); + + btVector3 fDrag(0, 0, 0); + btVector3 fLift(0, 0, 0); + + btScalar n_dot_v = nrm.dot(rel_v_nrm); + btScalar tri_area = 0.5f * f.m_ra; + + fDrag = 0.5f * kDG * medium.m_density * rel_v2 * tri_area * n_dot_v * (-rel_v_nrm); + + // Check angle of attack + // cos(10º) = 0.98480 + if ( 0 < n_dot_v && n_dot_v < 0.98480f) + fLift = 0.5f * kLF * medium.m_density * rel_v_len * tri_area * btSqrt(1.0f-n_dot_v*n_dot_v) * (nrm.cross(rel_v_nrm).cross(rel_v_nrm)); + + fDrag /= 3; + fLift /= 3; + + for(int j=0;j<3;++j) + { + if (f.m_n[j]->m_im>0) + { + f.m_n[j]->m_f += fDrag; + f.m_n[j]->m_f += fLift; + } + } + } + else if (m_cfg.aeromodel == btSoftBody::eAeroModel::F_OneSided || m_cfg.aeromodel == btSoftBody::eAeroModel::F_TwoSided) + { + if (btSoftBody::eAeroModel::F_TwoSided) + nrm *= (btScalar)( (btDot(nrm,rel_v) < 0) ? -1 : +1); + + const btScalar dvn=btDot(rel_v,nrm); + /* Compute forces */ + if(dvn>0) + { + btVector3 force(0,0,0); + const btScalar c0 = f.m_ra*dvn*rel_v2; + const btScalar c1 = c0*medium.m_density; + force += nrm*(-c1*kLF); + force += rel_v.normalized()*(-c1*kDG); + force /= 3; + for(int j=0;j<3;++j) ApplyClampedForce(*f.m_n[j],force,dt); + } + } + } + } + +} + // void btSoftBody::addVelocity(const btVector3& velocity) { @@ -1820,7 +1983,7 @@ btScalar btSoftBody::RayFromToCaster::rayFromToTriangle( const btVector3& rayF void btSoftBody::pointersToIndices() { #define PTR2IDX(_p_,_b_) reinterpret_cast<btSoftBody::Node*>((_p_)-(_b_)) - btSoftBody::Node* base=&m_nodes[0]; + btSoftBody::Node* base=m_nodes.size() ? &m_nodes[0] : 0; int i,ni; for(i=0,ni=m_nodes.size();i<ni;++i) @@ -1864,7 +2027,7 @@ void btSoftBody::indicesToPointers(const int* map) { #define IDX2PTR(_p_,_b_) map?(&(_b_)[map[(((char*)_p_)-(char*)0)]]): \ (&(_b_)[(((char*)_p_)-(char*)0)]) - btSoftBody::Node* base=&m_nodes[0]; + btSoftBody::Node* base=m_nodes.size() ? &m_nodes[0]:0; int i,ni; for(i=0,ni=m_nodes.size();i<ni;++i) @@ -1908,11 +2071,12 @@ int btSoftBody::rayTest(const btVector3& rayFrom,const btVector3& rayTo, btScalar& mint,eFeature::_& feature,int& index,bool bcountonly) const { int cnt=0; + btVector3 dir = rayTo-rayFrom; + + if(bcountonly||m_fdbvt.empty()) {/* Full search */ - btVector3 dir = rayTo-rayFrom; - dir.normalize(); - + for(int i=0,ni=m_faces.size();i<ni;++i) { const btSoftBody::Face& f=m_faces[i]; @@ -1947,6 +2111,37 @@ int btSoftBody::rayTest(const btVector3& rayFrom,const btVector3& rayTo, cnt=1; } } + + for (int i=0;i<m_tetras.size();i++) + { + const btSoftBody::Tetra& tet = m_tetras[i]; + int tetfaces[4][3] = {{0,1,2},{0,1,3},{1,2,3},{0,2,3}}; + for (int f=0;f<4;f++) + { + + int index0=tetfaces[f][0]; + int index1=tetfaces[f][1]; + int index2=tetfaces[f][2]; + btVector3 v0=tet.m_n[index0]->m_x; + btVector3 v1=tet.m_n[index1]->m_x; + btVector3 v2=tet.m_n[index2]->m_x; + + + const btScalar t=RayFromToCaster::rayFromToTriangle( rayFrom,rayTo,dir, + v0,v1,v2, + mint); + if(t>0) + { + ++cnt; + if(!bcountonly) + { + feature=btSoftBody::eFeature::Tetra; + index=i; + mint=t; + } + } + } + } return(cnt); } @@ -2660,44 +2855,8 @@ void btSoftBody::applyForces() { if(use_medium) { - EvaluateMedium(m_worldInfo, n.m_x, medium); - medium.m_velocity = m_windVelocity; - medium.m_density = m_worldInfo->air_density; - /* Aerodynamics */ - if(as_vaero) - { - const btVector3 rel_v = n.m_v - medium.m_velocity; - const btScalar rel_v2 = rel_v.length2(); - if(rel_v2>SIMD_EPSILON) - { - btVector3 nrm = n.m_n; - /* Setup normal */ - switch(m_cfg.aeromodel) - { - case btSoftBody::eAeroModel::V_Point: - nrm = NormalizeAny(rel_v); - break; - case btSoftBody::eAeroModel::V_TwoSided: - nrm *= (btScalar)( (btDot(nrm,rel_v) < 0) ? -1 : +1); - break; - default: - { - } - } - const btScalar dvn = btDot(rel_v,nrm); - /* Compute forces */ - if(dvn>0) - { - btVector3 force(0,0,0); - const btScalar c0 = n.m_area * dvn * rel_v2/2; - const btScalar c1 = c0 * medium.m_density; - force += nrm*(-c1*kLF); - force += rel_v.normalized() * (-c1 * kDG); - ApplyClampedForce(n, force, dt); - } - } - } + addAeroForceToNode(m_windVelocity, i); } /* Pressure */ if(as_pressure) @@ -2711,43 +2870,14 @@ void btSoftBody::applyForces() } } } + /* Per face forces */ for(i=0,ni=m_faces.size();i<ni;++i) { btSoftBody::Face& f=m_faces[i]; - if(as_faero) - { - const btVector3 v=(f.m_n[0]->m_v+f.m_n[1]->m_v+f.m_n[2]->m_v)/3; - const btVector3 x=(f.m_n[0]->m_x+f.m_n[1]->m_x+f.m_n[2]->m_x)/3; - EvaluateMedium(m_worldInfo,x,medium); - const btVector3 rel_v=v-medium.m_velocity; - const btScalar rel_v2=rel_v.length2(); - if(rel_v2>SIMD_EPSILON) - { - btVector3 nrm=f.m_normal; - /* Setup normal */ - switch(m_cfg.aeromodel) - { - case btSoftBody::eAeroModel::F_TwoSided: - nrm*=(btScalar)(btDot(nrm,rel_v)<0?-1:+1);break; - default: - { - } - } - const btScalar dvn=btDot(rel_v,nrm); - /* Compute forces */ - if(dvn>0) - { - btVector3 force(0,0,0); - const btScalar c0 = f.m_ra*dvn*rel_v2; - const btScalar c1 = c0*medium.m_density; - force += nrm*(-c1*kLF); - force += rel_v.normalized()*(-c1*kDG); - force /= 3; - for(int j=0;j<3;++j) ApplyClampedForce(*f.m_n[j],force,dt); - } - } - } + + /* Aerodynamics */ + addAeroForceToFace(m_windVelocity, i); } } @@ -2765,7 +2895,7 @@ void btSoftBody::PSolve_Anchors(btSoftBody* psb,btScalar kst,btScalar ti) const btVector3 va=a.m_body->getVelocityInLocalPoint(a.m_c1)*dt; const btVector3 vb=n.m_x-n.m_q; const btVector3 vr=(va-vb)+(wa-n.m_x)*kAHR; - const btVector3 impulse=a.m_c0*vr; + const btVector3 impulse=a.m_c0*vr*a.m_influence; n.m_x+=impulse*a.m_c2; a.m_body->applyImpulse(-impulse,a.m_c1); } @@ -3196,7 +3326,7 @@ const char* btSoftBody::serialize(void* dataBuffer, class btSerializer* serializ sbd->m_config.m_softRigidClusterImpulseSplit = m_cfg.kSR_SPLT_CL; sbd->m_config.m_softKineticClusterImpulseSplit = m_cfg.kSK_SPLT_CL; sbd->m_config.m_softSoftClusterImpulseSplit = m_cfg.kSS_SPLT_CL; - + //pose for shape matching { sbd->m_pose = (SoftBodyPoseData*)serializer->getUniquePointer((void*)&m_pose); diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBody.h b/extern/bullet2/src/BulletSoftBody/btSoftBody.h index ad8678f28b2..ba589486f26 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBody.h +++ b/extern/bullet2/src/BulletSoftBody/btSoftBody.h @@ -80,11 +80,13 @@ public: ///eAeroModel struct eAeroModel { enum _ { - V_Point, ///Vertex normals are oriented toward velocity - V_TwoSided, ///Vertex normals are fliped to match velocity - V_OneSided, ///Vertex normals are taken as it is - F_TwoSided, ///Face normals are fliped to match velocity - F_OneSided, ///Face normals are taken as it is + V_Point, ///Vertex normals are oriented toward velocity + V_TwoSided, ///Vertex normals are flipped to match velocity + V_TwoSidedLiftDrag, ///Vertex normals are flipped to match velocity and lift and drag forces are applied + V_OneSided, ///Vertex normals are taken as it is + F_TwoSided, ///Face normals are flipped to match velocity + F_TwoSidedLiftDrag, ///Face normals are flipped to match velocity and lift and drag forces are applied + F_OneSided, ///Face normals are taken as it is END };}; @@ -117,6 +119,7 @@ public: Node, Link, Face, + Tetra, END };}; @@ -282,6 +285,7 @@ public: Node* m_node; // Node pointer btVector3 m_local; // Anchor position in body space btRigidBody* m_body; // Body + btScalar m_influence; btMatrix3x3 m_c0; // Impulse matrix btVector3 m_c1; // Relative anchor btScalar m_c2; // ima*dt @@ -752,8 +756,8 @@ public: /* Append anchor */ void appendAnchor( int node, - btRigidBody* body, bool disableCollisionBetweenLinkedBodies=false); - void appendAnchor(int node,btRigidBody* body, const btVector3& localPivot,bool disableCollisionBetweenLinkedBodies=false); + btRigidBody* body, bool disableCollisionBetweenLinkedBodies=false,btScalar influence = 1); + void appendAnchor(int node,btRigidBody* body, const btVector3& localPivot,bool disableCollisionBetweenLinkedBodies=false,btScalar influence = 1); /* Append linear joint */ void appendLinearJoint(const LJoint::Specs& specs,Cluster* body0,Body body1); void appendLinearJoint(const LJoint::Specs& specs,Body body=Body()); @@ -767,6 +771,12 @@ public: /* Add force (or gravity) to a node of the body */ void addForce( const btVector3& force, int node); + /* Add aero force to a node of the body */ + void addAeroForceToNode(const btVector3& windVelocity,int nodeIndex); + + /* Add aero force to a face of the body */ + void addAeroForceToFace(const btVector3& windVelocity,int faceIndex); + /* Add velocity to the entire body */ void addVelocity( const btVector3& velocity); diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp b/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp index 04ee7ea77cf..d99be3b8138 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp +++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.cpp @@ -95,7 +95,7 @@ void btSoftBodyTriangleCallback::processTriangle(btVector3* triangle,int partId, ci.m_dispatcher1 = m_dispatcher; ///debug drawing of the overlapping triangles - if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && m_dispatchInfoPtr->m_debugDraw->getDebugMode() &btIDebugDraw::DBG_DrawWireframe) + if (m_dispatchInfoPtr && m_dispatchInfoPtr->m_debugDraw && (m_dispatchInfoPtr->m_debugDraw->getDebugMode() &btIDebugDraw::DBG_DrawWireframe)) { btVector3 color(1,1,0); btTransform& tr = ob->getWorldTransform(); diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h b/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h index f311419d4a8..8a09fff2a37 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyConcaveCollisionAlgorithm.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef SOFT_BODY_CONCAVE_COLLISION_ALGORITHM_H -#define SOFT_BODY_CONCAVE_COLLISION_ALGORITHM_H +#ifndef BT_SOFT_BODY_CONCAVE_COLLISION_ALGORITHM_H +#define BT_SOFT_BODY_CONCAVE_COLLISION_ALGORITHM_H #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btDispatcher.h" @@ -152,4 +152,4 @@ public: }; -#endif //SOFT_BODY_CONCAVE_COLLISION_ALGORITHM_H +#endif //BT_SOFT_BODY_CONCAVE_COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp b/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp index 1a271066497..0fb3560e94c 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp +++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.cpp @@ -19,6 +19,8 @@ subject to the following restrictions: #include <string.h> #include "btSoftBodyHelpers.h" #include "LinearMath/btConvexHull.h" +#include "LinearMath/btConvexHullComputer.h" + // static void drawVertex( btIDebugDraw* idraw, @@ -183,6 +185,35 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, { vertices[j]=psb->m_clusters[i]->m_nodes[j]->m_x; } +#define USE_NEW_CONVEX_HULL_COMPUTER +#ifdef USE_NEW_CONVEX_HULL_COMPUTER + btConvexHullComputer computer; + int stride = sizeof(btVector3); + int count = vertices.size(); + btScalar shrink=0.f; + btScalar shrinkClamp=0.f; + computer.compute(&vertices[0].getX(),stride,count,shrink,shrinkClamp); + for (int i=0;i<computer.faces.size();i++) + { + + int face = computer.faces[i]; + //printf("face=%d\n",face); + const btConvexHullComputer::Edge* firstEdge = &computer.edges[face]; + const btConvexHullComputer::Edge* edge = firstEdge->getNextEdgeOfFace(); + + int v0 = firstEdge->getSourceVertex(); + int v1 = firstEdge->getTargetVertex(); + while (edge!=firstEdge) + { + int v2 = edge->getTargetVertex(); + idraw->drawTriangle(computer.vertices[v0],computer.vertices[v1],computer.vertices[v2],color,1); + edge = edge->getNextEdgeOfFace(); + v0=v1; + v1=v2; + }; + } +#else + HullDesc hdsc(QF_TRIANGLES,vertices.size(),&vertices[0]); HullResult hres; HullLibrary hlib; @@ -201,6 +232,8 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, color,1); } hlib.ReleaseResult(hres); +#endif + } /* Velocities */ #if 0 @@ -296,7 +329,7 @@ void btSoftBodyHelpers::Draw( btSoftBody* psb, { const btScalar scl=(btScalar)0.8; const btScalar alp=(btScalar)1; - const btVector3 col((btScalar)0.7,(btScalar)0.7,(btScalar)0.7); + const btVector3 col((btScalar)0.3,(btScalar)0.3,(btScalar)0.7); for(int i=0;i<psb->m_tetras.size();++i) { const btSoftBody::Tetra& t=psb->m_tetras[i]; diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.h b/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.h index 49b1f0bbce8..620a52fe394 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.h +++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyHelpers.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef SOFT_BODY_HELPERS_H -#define SOFT_BODY_HELPERS_H +#ifndef BT_SOFT_BODY_HELPERS_H +#define BT_SOFT_BODY_HELPERS_H #include "btSoftBody.h" @@ -140,4 +140,4 @@ struct btSoftBodyHelpers }; -#endif //SOFT_BODY_HELPERS_H +#endif //BT_SOFT_BODY_HELPERS_H diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodyInternals.h b/extern/bullet2/src/BulletSoftBody/btSoftBodyInternals.h index 885571069d0..5ef8db19396 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBodyInternals.h +++ b/extern/bullet2/src/BulletSoftBody/btSoftBodyInternals.h @@ -70,7 +70,7 @@ public: ///getAabb returns the axis aligned bounding box in the coordinate frame of the given transform t. virtual void getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const { - /* t should be identity, but better be safe than...fast? */ + /* t is usually identity, except when colliding against btCompoundShape. See Issue 512 */ const btVector3 mins=m_body->m_bounds[0]; const btVector3 maxs=m_body->m_bounds[1]; const btVector3 crns[]={t*btVector3(mins.x(),mins.y(),mins.z()), diff --git a/extern/bullet2/src/BulletSoftBody/btSoftBodySolvers.h b/extern/bullet2/src/BulletSoftBody/btSoftBodySolvers.h index 440084c5f7b..2fcd8b67616 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftBodySolvers.h +++ b/extern/bullet2/src/BulletSoftBody/btSoftBodySolvers.h @@ -73,7 +73,7 @@ public: virtual void optimize( btAlignedObjectArray< btSoftBody * > &softBodies , bool forceUpdate=false) = 0; /** Copy necessary data back to the original soft body source objects. */ - virtual void copyBackToSoftBodies() = 0; + virtual void copyBackToSoftBodies(bool bMove = true) = 0; /** Predict motion of soft bodies into next timestep */ virtual void predictMotion( float solverdt ) = 0; diff --git a/extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h b/extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h index adc3844e363..7658e3c2252 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletSoftBody/btSoftRigidCollisionAlgorithm.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef SOFT_RIGID_COLLISION_ALGORITHM_H -#define SOFT_RIGID_COLLISION_ALGORITHM_H +#ifndef BT_SOFT_RIGID_COLLISION_ALGORITHM_H +#define BT_SOFT_RIGID_COLLISION_ALGORITHM_H #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" @@ -70,6 +70,6 @@ public: }; -#endif //SOFT_RIGID_COLLISION_ALGORITHM_H +#endif //BT_SOFT_RIGID_COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp b/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp index 1b9b5e392a1..8f4be231c13 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp +++ b/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.cpp @@ -165,7 +165,7 @@ void btSoftRigidDynamicsWorld::debugDrawWorld() for ( i=0;i<this->m_softBodies.size();i++) { btSoftBody* psb=(btSoftBody*)this->m_softBodies[i]; - if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe)) + if (getDebugDrawer() && (getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe))) { btSoftBodyHelpers::DrawFrame(psb,m_debugDrawer); btSoftBodyHelpers::Draw(psb,m_debugDrawer,m_drawFlags); @@ -300,12 +300,19 @@ void btSoftRigidDynamicsWorld::rayTestSingle(const btTransform& rayFromTrans,con shapeInfo.m_shapePart = 0; shapeInfo.m_triangleIndex = softResult.index; // get the normal - btVector3 normal = softBody->m_faces[softResult.index].m_normal; btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin(); - if (normal.dot(rayDir) > 0) { - // normal always point toward origin of the ray - normal = -normal; + btVector3 normal=-rayDir; + normal.normalize(); + + if (softResult.feature == btSoftBody::eFeature::Face) + { + normal = softBody->m_faces[softResult.index].m_normal; + if (normal.dot(rayDir) > 0) { + // normal always point toward origin of the ray + normal = -normal; + } } + btCollisionWorld::LocalRayResult rayResult (collisionObject, &shapeInfo, diff --git a/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.h b/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.h index 7d8d0cb7daa..3e0efafd6c7 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.h +++ b/extern/bullet2/src/BulletSoftBody/btSoftRigidDynamicsWorld.h @@ -73,6 +73,10 @@ public: return m_sbi; } + virtual btDynamicsWorldType getWorldType() const + { + return BT_SOFT_RIGID_DYNAMICS_WORLD; + } btSoftBodyArray& getSoftBodyArray() { diff --git a/extern/bullet2/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h b/extern/bullet2/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h index 1b34e0af60f..92d683c1dc8 100644 --- a/extern/bullet2/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h +++ b/extern/bullet2/src/BulletSoftBody/btSoftSoftCollisionAlgorithm.h @@ -13,8 +13,8 @@ subject to the following restrictions: 3. This notice may not be removed or altered from any source distribution. */ -#ifndef SOFT_SOFT_COLLISION_ALGORITHM_H -#define SOFT_SOFT_COLLISION_ALGORITHM_H +#ifndef BT_SOFT_SOFT_COLLISION_ALGORITHM_H +#define BT_SOFT_SOFT_COLLISION_ALGORITHM_H #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h" #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h" @@ -64,6 +64,6 @@ public: }; -#endif //SOFT_SOFT_COLLISION_ALGORITHM_H +#endif //BT_SOFT_SOFT_COLLISION_ALGORITHM_H diff --git a/extern/bullet2/src/BulletSoftBody/btSparseSDF.h b/extern/bullet2/src/BulletSoftBody/btSparseSDF.h index cc4266732ae..90a26cdf7e3 100644 --- a/extern/bullet2/src/BulletSoftBody/btSparseSDF.h +++ b/extern/bullet2/src/BulletSoftBody/btSparseSDF.h @@ -14,8 +14,8 @@ subject to the following restrictions: */ ///btSparseSdf implementation by Nathanael Presson -#ifndef _14F9D17F_EAE8_4aba_B41C_292DB2AA70F3_ -#define _14F9D17F_EAE8_4aba_B41C_292DB2AA70F3_ +#ifndef BT_SPARSE_SDF_H +#define BT_SPARSE_SDF_H #include "BulletCollision/CollisionDispatch/btCollisionObject.h" #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h" @@ -303,4 +303,4 @@ struct btSparseSdf }; -#endif +#endif //BT_SPARSE_SDF_H diff --git a/extern/bullet2/src/LinearMath/btAabbUtil2.h b/extern/bullet2/src/LinearMath/btAabbUtil2.h index 532ce1bf633..42b721dea22 100644 --- a/extern/bullet2/src/LinearMath/btAabbUtil2.h +++ b/extern/bullet2/src/LinearMath/btAabbUtil2.h @@ -14,8 +14,8 @@ subject to the following restrictions: -#ifndef AABB_UTIL2 -#define AABB_UTIL2 +#ifndef BT_AABB_UTIL2 +#define BT_AABB_UTIL2 #include "btTransform.h" #include "btVector3.h" @@ -231,6 +231,6 @@ SIMD_FORCE_INLINE void btTransformAabb(const btVector3& localAabbMin,const btVec } #endif //USE_BANCHLESS -#endif +#endif //BT_AABB_UTIL2 diff --git a/extern/bullet2/src/LinearMath/btAlignedAllocator.cpp b/extern/bullet2/src/LinearMath/btAlignedAllocator.cpp index 189b759d237..c4c0ceb2ed2 100644 --- a/extern/bullet2/src/LinearMath/btAlignedAllocator.cpp +++ b/extern/bullet2/src/LinearMath/btAlignedAllocator.cpp @@ -58,16 +58,18 @@ static inline void btAlignedFreeDefault(void *ptr) free(ptr); } #else + + + + + static inline void *btAlignedAllocDefault(size_t size, int alignment) { void *ret; char *real; - unsigned long offset; - real = (char *)sAllocFunc(size + sizeof(void *) + (alignment-1)); if (real) { - offset = (alignment - (unsigned long)(real + sizeof(void *))) & (alignment-1); - ret = (void *)((real + sizeof(void *)) + offset); + ret = btAlignPointer(real + sizeof(void *),alignment); *((void **)(ret)-1) = (void *)(real); } else { ret = (void *)(real); @@ -110,7 +112,6 @@ void* btAlignedAllocInternal (size_t size, int alignment,int line,char* filen { void *ret; char *real; - unsigned long offset; gTotalBytesAlignedAllocs += size; gNumAlignedAllocs++; @@ -118,9 +119,7 @@ void* btAlignedAllocInternal (size_t size, int alignment,int line,char* filen real = (char *)sAllocFunc(size + 2*sizeof(void *) + (alignment-1)); if (real) { - offset = (alignment - (unsigned long)(real + 2*sizeof(void *))) & -(alignment-1); - ret = (void *)((real + 2*sizeof(void *)) + offset); + ret = (void*) btAlignPointer((real + 2*sizeof(void *), alignment); *((void **)(ret)-1) = (void *)(real); *((int*)(ret)-2) = size; diff --git a/extern/bullet2/src/LinearMath/btAlignedObjectArray.h b/extern/bullet2/src/LinearMath/btAlignedObjectArray.h index 955bb128e83..36090e13c89 100644 --- a/extern/bullet2/src/LinearMath/btAlignedObjectArray.h +++ b/extern/bullet2/src/LinearMath/btAlignedObjectArray.h @@ -28,6 +28,7 @@ subject to the following restrictions: #define BT_USE_PLACEMENT_NEW 1 //#define BT_USE_MEMCPY 1 //disable, because it is cumbersome to find out for each platform where memcpy is defined. It can be in <memory.h> or <string.h> or otherwise... +#define BT_ALLOW_ARRAY_COPY_OPERATOR // enabling this can accidently perform deep copies of data if you are not careful #ifdef BT_USE_MEMCPY #include <memory.h> @@ -53,7 +54,19 @@ class btAlignedObjectArray //PCK: added this line bool m_ownsMemory; - protected: +#ifdef BT_ALLOW_ARRAY_COPY_OPERATOR +public: + SIMD_FORCE_INLINE btAlignedObjectArray<T>& operator=(const btAlignedObjectArray<T> &other) + { + copyFromArray(other); + return *this; + } +#else//BT_ALLOW_ARRAY_COPY_OPERATOR +private: + SIMD_FORCE_INLINE btAlignedObjectArray<T>& operator=(const btAlignedObjectArray<T> &other); +#endif//BT_ALLOW_ARRAY_COPY_OPERATOR + +protected: SIMD_FORCE_INLINE int allocSize(int size) { return (size ? size*2 : 1); @@ -140,21 +153,29 @@ class btAlignedObjectArray SIMD_FORCE_INLINE const T& at(int n) const { + btAssert(n>=0); + btAssert(n<size()); return m_data[n]; } SIMD_FORCE_INLINE T& at(int n) { + btAssert(n>=0); + btAssert(n<size()); return m_data[n]; } SIMD_FORCE_INLINE const T& operator[](int n) const { + btAssert(n>=0); + btAssert(n<size()); return m_data[n]; } SIMD_FORCE_INLINE T& operator[](int n) { + btAssert(n>=0); + btAssert(n<size()); return m_data[n]; } @@ -171,6 +192,7 @@ class btAlignedObjectArray SIMD_FORCE_INLINE void pop_back() { + btAssert(m_size>0); m_size--; m_data[m_size].~T(); } @@ -291,8 +313,9 @@ class btAlignedObjectArray } }; + template <typename L> - void quickSortInternal(L CompareFunc,int lo, int hi) + void quickSortInternal(const L& CompareFunc,int lo, int hi) { // lo is the lower index, hi is the upper index // of the region of array a that is to be sorted @@ -322,7 +345,7 @@ class btAlignedObjectArray template <typename L> - void quickSort(L CompareFunc) + void quickSort(const L& CompareFunc) { //don't sort 0 or 1 elements if (size()>1) @@ -334,7 +357,7 @@ class btAlignedObjectArray ///heap sort from http://www.csse.monash.edu.au/~lloyd/tildeAlgDS/Sort/Heap/ template <typename L> - void downHeap(T *pArr, int k, int n,L CompareFunc) + void downHeap(T *pArr, int k, int n, const L& CompareFunc) { /* PRE: a[k+1..N] is a heap */ /* POST: a[k..N] is a heap */ @@ -380,7 +403,7 @@ class btAlignedObjectArray } template <typename L> - void heapSort(L CompareFunc) + void heapSort(const L& CompareFunc) { /* sort a[0..N-1], N.B. 0 to N-1 */ int k; diff --git a/extern/bullet2/src/LinearMath/btConvexHull.h b/extern/bullet2/src/LinearMath/btConvexHull.h index a23fa4d550f..69c52bc6f83 100644 --- a/extern/bullet2/src/LinearMath/btConvexHull.h +++ b/extern/bullet2/src/LinearMath/btConvexHull.h @@ -16,8 +16,8 @@ subject to the following restrictions: ///includes modifications/improvements by John Ratcliff, see BringOutYourDead below. -#ifndef CD_HULL_H -#define CD_HULL_H +#ifndef BT_CD_HULL_H +#define BT_CD_HULL_H #include "btVector3.h" #include "btAlignedObjectArray.h" @@ -237,5 +237,5 @@ private: }; -#endif +#endif //BT_CD_HULL_H diff --git a/extern/bullet2/src/LinearMath/btConvexHullComputer.cpp b/extern/bullet2/src/LinearMath/btConvexHullComputer.cpp new file mode 100644 index 00000000000..c03c901c051 --- /dev/null +++ b/extern/bullet2/src/LinearMath/btConvexHullComputer.cpp @@ -0,0 +1,2751 @@ +/* +Copyright (c) 2011 Ole Kniemeyer, MAXON, www.maxon.net + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#include <string.h> + +#include "btConvexHullComputer.h" +#include "btAlignedObjectArray.h" +#include "btMinMax.h" +#include "btVector3.h" + +#ifdef __GNUC__ + #include <stdint.h> +#elif defined(_MSC_VER) + typedef __int32 int32_t; + typedef __int64 int64_t; + typedef unsigned __int32 uint32_t; + typedef unsigned __int64 uint64_t; +#else + typedef int int32_t; + typedef long long int int64_t; + typedef unsigned int uint32_t; + typedef unsigned long long int uint64_t; +#endif + + +//The definition of USE_X86_64_ASM is moved into the build system. You can enable it manually by commenting out the following lines +//#if (defined(__GNUC__) && defined(__x86_64__) && !defined(__ICL)) // || (defined(__ICL) && defined(_M_X64)) bug in Intel compiler, disable inline assembly +// #define USE_X86_64_ASM +//#endif + + +//#define DEBUG_CONVEX_HULL +//#define SHOW_ITERATIONS + +#if defined(DEBUG_CONVEX_HULL) || defined(SHOW_ITERATIONS) + #include <stdio.h> +#endif + +// Convex hull implementation based on Preparata and Hong +// Ole Kniemeyer, MAXON Computer GmbH +class btConvexHullInternal +{ + public: + + class Point64 + { + public: + int64_t x; + int64_t y; + int64_t z; + + Point64(int64_t x, int64_t y, int64_t z): x(x), y(y), z(z) + { + } + + bool isZero() + { + return (x == 0) && (y == 0) && (z == 0); + } + + int64_t dot(const Point64& b) const + { + return x * b.x + y * b.y + z * b.z; + } + }; + + class Point32 + { + public: + int32_t x; + int32_t y; + int32_t z; + int index; + + Point32() + { + } + + Point32(int32_t x, int32_t y, int32_t z): x(x), y(y), z(z), index(-1) + { + } + + bool operator==(const Point32& b) const + { + return (x == b.x) && (y == b.y) && (z == b.z); + } + + bool operator!=(const Point32& b) const + { + return (x != b.x) || (y != b.y) || (z != b.z); + } + + bool isZero() + { + return (x == 0) && (y == 0) && (z == 0); + } + + Point64 cross(const Point32& b) const + { + return Point64(y * b.z - z * b.y, z * b.x - x * b.z, x * b.y - y * b.x); + } + + Point64 cross(const Point64& b) const + { + return Point64(y * b.z - z * b.y, z * b.x - x * b.z, x * b.y - y * b.x); + } + + int64_t dot(const Point32& b) const + { + return x * b.x + y * b.y + z * b.z; + } + + int64_t dot(const Point64& b) const + { + return x * b.x + y * b.y + z * b.z; + } + + Point32 operator+(const Point32& b) const + { + return Point32(x + b.x, y + b.y, z + b.z); + } + + Point32 operator-(const Point32& b) const + { + return Point32(x - b.x, y - b.y, z - b.z); + } + }; + + class Int128 + { + public: + uint64_t low; + uint64_t high; + + Int128() + { + } + + Int128(uint64_t low, uint64_t high): low(low), high(high) + { + } + + Int128(uint64_t low): low(low), high(0) + { + } + + Int128(int64_t value): low(value), high((value >= 0) ? 0 : (uint64_t) -1LL) + { + } + + static Int128 mul(int64_t a, int64_t b); + + static Int128 mul(uint64_t a, uint64_t b); + + Int128 operator-() const + { + return Int128((uint64_t) -(int64_t)low, ~high + (low == 0)); + } + + Int128 operator+(const Int128& b) const + { +#ifdef USE_X86_64_ASM + Int128 result; + __asm__ ("addq %[bl], %[rl]\n\t" + "adcq %[bh], %[rh]\n\t" + : [rl] "=r" (result.low), [rh] "=r" (result.high) + : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high) + : "cc" ); + return result; +#else + uint64_t lo = low + b.low; + return Int128(lo, high + b.high + (lo < low)); +#endif + } + + Int128 operator-(const Int128& b) const + { +#ifdef USE_X86_64_ASM + Int128 result; + __asm__ ("subq %[bl], %[rl]\n\t" + "sbbq %[bh], %[rh]\n\t" + : [rl] "=r" (result.low), [rh] "=r" (result.high) + : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high) + : "cc" ); + return result; +#else + return *this + -b; +#endif + } + + Int128& operator+=(const Int128& b) + { +#ifdef USE_X86_64_ASM + __asm__ ("addq %[bl], %[rl]\n\t" + "adcq %[bh], %[rh]\n\t" + : [rl] "=r" (low), [rh] "=r" (high) + : "0"(low), "1"(high), [bl] "g"(b.low), [bh] "g"(b.high) + : "cc" ); +#else + uint64_t lo = low + b.low; + if (lo < low) + { + ++high; + } + low = lo; + high += b.high; +#endif + return *this; + } + + Int128& operator++() + { + if (++low == 0) + { + ++high; + } + return *this; + } + + Int128 operator*(int64_t b) const; + + btScalar toScalar() const + { + return ((int64_t) high >= 0) ? btScalar(high) * (btScalar(0x100000000LL) * btScalar(0x100000000LL)) + btScalar(low) + : -(-*this).toScalar(); + } + + int getSign() const + { + return ((int64_t) high < 0) ? -1 : (high || low) ? 1 : 0; + } + + bool operator<(const Int128& b) const + { + return (high < b.high) || ((high == b.high) && (low < b.low)); + } + + int ucmp(const Int128&b) const + { + if (high < b.high) + { + return -1; + } + if (high > b.high) + { + return 1; + } + if (low < b.low) + { + return -1; + } + if (low > b.low) + { + return 1; + } + return 0; + } + }; + + + class Rational64 + { + private: + uint64_t m_numerator; + uint64_t m_denominator; + int sign; + + public: + Rational64(int64_t numerator, int64_t denominator) + { + if (numerator > 0) + { + sign = 1; + m_numerator = (uint64_t) numerator; + } + else if (numerator < 0) + { + sign = -1; + m_numerator = (uint64_t) -numerator; + } + else + { + sign = 0; + m_numerator = 0; + } + if (denominator > 0) + { + m_denominator = (uint64_t) denominator; + } + else if (denominator < 0) + { + sign = -sign; + m_denominator = (uint64_t) -denominator; + } + else + { + m_denominator = 0; + } + } + + bool isNegativeInfinity() const + { + return (sign < 0) && (m_denominator == 0); + } + + bool isNaN() const + { + return (sign == 0) && (m_denominator == 0); + } + + int compare(const Rational64& b) const; + + btScalar toScalar() const + { + return sign * ((m_denominator == 0) ? SIMD_INFINITY : (btScalar) m_numerator / m_denominator); + } + }; + + + class Rational128 + { + private: + Int128 numerator; + Int128 denominator; + int sign; + bool isInt64; + + public: + Rational128(int64_t value) + { + if (value > 0) + { + sign = 1; + this->numerator = value; + } + else if (value < 0) + { + sign = -1; + this->numerator = -value; + } + else + { + sign = 0; + this->numerator = (uint64_t) 0; + } + this->denominator = (uint64_t) 1; + isInt64 = true; + } + + Rational128(const Int128& numerator, const Int128& denominator) + { + sign = numerator.getSign(); + if (sign >= 0) + { + this->numerator = numerator; + } + else + { + this->numerator = -numerator; + } + int dsign = denominator.getSign(); + if (dsign >= 0) + { + this->denominator = denominator; + } + else + { + sign = -sign; + this->denominator = -denominator; + } + isInt64 = false; + } + + int compare(const Rational128& b) const; + + int compare(int64_t b) const; + + btScalar toScalar() const + { + return sign * ((denominator.getSign() == 0) ? SIMD_INFINITY : numerator.toScalar() / denominator.toScalar()); + } + }; + + class PointR128 + { + public: + Int128 x; + Int128 y; + Int128 z; + Int128 denominator; + + PointR128() + { + } + + PointR128(Int128 x, Int128 y, Int128 z, Int128 denominator): x(x), y(y), z(z), denominator(denominator) + { + } + + btScalar xvalue() const + { + return x.toScalar() / denominator.toScalar(); + } + + btScalar yvalue() const + { + return y.toScalar() / denominator.toScalar(); + } + + btScalar zvalue() const + { + return z.toScalar() / denominator.toScalar(); + } + }; + + + class Edge; + class Face; + + class Vertex + { + public: + Vertex* next; + Vertex* prev; + Edge* edges; + Face* firstNearbyFace; + Face* lastNearbyFace; + PointR128 point128; + Point32 point; + int copy; + + Vertex(): next(NULL), prev(NULL), edges(NULL), firstNearbyFace(NULL), lastNearbyFace(NULL), copy(-1) + { + } + +#ifdef DEBUG_CONVEX_HULL + void print() + { + printf("V%d (%d, %d, %d)", point.index, point.x, point.y, point.z); + } + + void printGraph(); +#endif + + Point32 operator-(const Vertex& b) const + { + return point - b.point; + } + + Rational128 dot(const Point64& b) const + { + return (point.index >= 0) ? Rational128(point.dot(b)) + : Rational128(point128.x * b.x + point128.y * b.y + point128.z * b.z, point128.denominator); + } + + btScalar xvalue() const + { + return (point.index >= 0) ? btScalar(point.x) : point128.xvalue(); + } + + btScalar yvalue() const + { + return (point.index >= 0) ? btScalar(point.y) : point128.yvalue(); + } + + btScalar zvalue() const + { + return (point.index >= 0) ? btScalar(point.z) : point128.zvalue(); + } + + void receiveNearbyFaces(Vertex* src) + { + if (lastNearbyFace) + { + lastNearbyFace->nextWithSameNearbyVertex = src->firstNearbyFace; + } + else + { + firstNearbyFace = src->firstNearbyFace; + } + if (src->lastNearbyFace) + { + lastNearbyFace = src->lastNearbyFace; + } + for (Face* f = src->firstNearbyFace; f; f = f->nextWithSameNearbyVertex) + { + btAssert(f->nearbyVertex == src); + f->nearbyVertex = this; + } + src->firstNearbyFace = NULL; + src->lastNearbyFace = NULL; + } + }; + + + class Edge + { + public: + Edge* next; + Edge* prev; + Edge* reverse; + Vertex* target; + Face* face; + int copy; + + ~Edge() + { + next = NULL; + prev = NULL; + reverse = NULL; + target = NULL; + face = NULL; + } + + void link(Edge* n) + { + btAssert(reverse->target == n->reverse->target); + next = n; + n->prev = this; + } + +#ifdef DEBUG_CONVEX_HULL + void print() + { + printf("E%p : %d -> %d, n=%p p=%p (0 %d\t%d\t%d) -> (%d %d %d)", this, reverse->target->point.index, target->point.index, next, prev, + reverse->target->point.x, reverse->target->point.y, reverse->target->point.z, target->point.x, target->point.y, target->point.z); + } +#endif + }; + + class Face + { + public: + Face* next; + Vertex* nearbyVertex; + Face* nextWithSameNearbyVertex; + Point32 origin; + Point32 dir0; + Point32 dir1; + + Face(): next(NULL), nearbyVertex(NULL), nextWithSameNearbyVertex(NULL) + { + } + + void init(Vertex* a, Vertex* b, Vertex* c) + { + nearbyVertex = a; + origin = a->point; + dir0 = *b - *a; + dir1 = *c - *a; + if (a->lastNearbyFace) + { + a->lastNearbyFace->nextWithSameNearbyVertex = this; + } + else + { + a->firstNearbyFace = this; + } + a->lastNearbyFace = this; + } + + Point64 getNormal() + { + return dir0.cross(dir1); + } + }; + + template<typename UWord, typename UHWord> class DMul + { + private: + static uint32_t high(uint64_t value) + { + return (uint32_t) (value >> 32); + } + + static uint32_t low(uint64_t value) + { + return (uint32_t) value; + } + + static uint64_t mul(uint32_t a, uint32_t b) + { + return (uint64_t) a * (uint64_t) b; + } + + static void shlHalf(uint64_t& value) + { + value <<= 32; + } + + static uint64_t high(Int128 value) + { + return value.high; + } + + static uint64_t low(Int128 value) + { + return value.low; + } + + static Int128 mul(uint64_t a, uint64_t b) + { + return Int128::mul(a, b); + } + + static void shlHalf(Int128& value) + { + value.high = value.low; + value.low = 0; + } + + public: + + static void mul(UWord a, UWord b, UWord& resLow, UWord& resHigh) + { + UWord p00 = mul(low(a), low(b)); + UWord p01 = mul(low(a), high(b)); + UWord p10 = mul(high(a), low(b)); + UWord p11 = mul(high(a), high(b)); + UWord p0110 = UWord(low(p01)) + UWord(low(p10)); + p11 += high(p01); + p11 += high(p10); + p11 += high(p0110); + shlHalf(p0110); + p00 += p0110; + if (p00 < p0110) + { + ++p11; + } + resLow = p00; + resHigh = p11; + } + }; + + private: + + class IntermediateHull + { + public: + Vertex* minXy; + Vertex* maxXy; + Vertex* minYx; + Vertex* maxYx; + + IntermediateHull(): minXy(NULL), maxXy(NULL), minYx(NULL), maxYx(NULL) + { + } + + void print(); + }; + + enum Orientation {NONE, CLOCKWISE, COUNTER_CLOCKWISE}; + + template <typename T> class PoolArray + { + private: + T* array; + int size; + + public: + PoolArray<T>* next; + + PoolArray(int size): size(size), next(NULL) + { + array = (T*) btAlignedAlloc(sizeof(T) * size, 16); + } + + ~PoolArray() + { + btAlignedFree(array); + } + + T* init() + { + T* o = array; + for (int i = 0; i < size; i++, o++) + { + o->next = (i+1 < size) ? o + 1 : NULL; + } + return array; + } + }; + + template <typename T> class Pool + { + private: + PoolArray<T>* arrays; + PoolArray<T>* nextArray; + T* freeObjects; + int arraySize; + + public: + Pool(): arrays(NULL), nextArray(NULL), freeObjects(NULL), arraySize(256) + { + } + + ~Pool() + { + while (arrays) + { + PoolArray<T>* p = arrays; + arrays = p->next; + p->~PoolArray<T>(); + btAlignedFree(p); + } + } + + void reset() + { + nextArray = arrays; + freeObjects = NULL; + } + + void setArraySize(int arraySize) + { + this->arraySize = arraySize; + } + + T* newObject() + { + T* o = freeObjects; + if (!o) + { + PoolArray<T>* p = nextArray; + if (p) + { + nextArray = p->next; + } + else + { + p = new(btAlignedAlloc(sizeof(PoolArray<T>), 16)) PoolArray<T>(arraySize); + p->next = arrays; + arrays = p; + } + o = p->init(); + } + freeObjects = o->next; + return new(o) T(); + }; + + void freeObject(T* object) + { + object->~T(); + object->next = freeObjects; + freeObjects = object; + } + }; + + btVector3 scaling; + btVector3 center; + Pool<Vertex> vertexPool; + Pool<Edge> edgePool; + Pool<Face> facePool; + btAlignedObjectArray<Vertex*> originalVertices; + int mergeStamp; + int minAxis; + int medAxis; + int maxAxis; + int usedEdgePairs; + int maxUsedEdgePairs; + + static Orientation getOrientation(const Edge* prev, const Edge* next, const Point32& s, const Point32& t); + Edge* findMaxAngle(bool ccw, const Vertex* start, const Point32& s, const Point64& rxs, const Point64& sxrxs, Rational64& minCot); + void findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge*& e0, Edge*& e1, Vertex* stop0, Vertex* stop1); + + Edge* newEdgePair(Vertex* from, Vertex* to); + + void removeEdgePair(Edge* edge) + { + Edge* n = edge->next; + Edge* r = edge->reverse; + + btAssert(edge->target && r->target); + + if (n != edge) + { + n->prev = edge->prev; + edge->prev->next = n; + r->target->edges = n; + } + else + { + r->target->edges = NULL; + } + + n = r->next; + + if (n != r) + { + n->prev = r->prev; + r->prev->next = n; + edge->target->edges = n; + } + else + { + edge->target->edges = NULL; + } + + edgePool.freeObject(edge); + edgePool.freeObject(r); + usedEdgePairs--; + } + + void computeInternal(int start, int end, IntermediateHull& result); + + bool mergeProjection(IntermediateHull& h0, IntermediateHull& h1, Vertex*& c0, Vertex*& c1); + + void merge(IntermediateHull& h0, IntermediateHull& h1); + + btVector3 toBtVector(const Point32& v); + + btVector3 getBtNormal(Face* face); + + bool shiftFace(Face* face, btScalar amount, btAlignedObjectArray<Vertex*> stack); + + public: + Vertex* vertexList; + + void compute(const void* coords, bool doubleCoords, int stride, int count); + + btVector3 getCoordinates(const Vertex* v); + + btScalar shrink(btScalar amount, btScalar clampAmount); +}; + + +btConvexHullInternal::Int128 btConvexHullInternal::Int128::operator*(int64_t b) const +{ + bool negative = (int64_t) high < 0; + Int128 a = negative ? -*this : *this; + if (b < 0) + { + negative = !negative; + b = -b; + } + Int128 result = mul(a.low, (uint64_t) b); + result.high += a.high * (uint64_t) b; + return negative ? -result : result; +} + +btConvexHullInternal::Int128 btConvexHullInternal::Int128::mul(int64_t a, int64_t b) +{ + Int128 result; + +#ifdef USE_X86_64_ASM + __asm__ ("imulq %[b]" + : "=a" (result.low), "=d" (result.high) + : "0"(a), [b] "r"(b) + : "cc" ); + return result; + +#else + bool negative = a < 0; + if (negative) + { + a = -a; + } + if (b < 0) + { + negative = !negative; + b = -b; + } + DMul<uint64_t, uint32_t>::mul((uint64_t) a, (uint64_t) b, result.low, result.high); + return negative ? -result : result; +#endif +} + +btConvexHullInternal::Int128 btConvexHullInternal::Int128::mul(uint64_t a, uint64_t b) +{ + Int128 result; + +#ifdef USE_X86_64_ASM + __asm__ ("mulq %[b]" + : "=a" (result.low), "=d" (result.high) + : "0"(a), [b] "r"(b) + : "cc" ); + +#else + DMul<uint64_t, uint32_t>::mul(a, b, result.low, result.high); +#endif + + return result; +} + +int btConvexHullInternal::Rational64::compare(const Rational64& b) const +{ + if (sign != b.sign) + { + return sign - b.sign; + } + else if (sign == 0) + { + return 0; + } + + // return (numerator * b.denominator > b.numerator * denominator) ? sign : (numerator * b.denominator < b.numerator * denominator) ? -sign : 0; + +#ifdef USE_X86_64_ASM + + int result; + int64_t tmp; + int64_t dummy; + __asm__ ("mulq %[bn]\n\t" + "movq %%rax, %[tmp]\n\t" + "movq %%rdx, %%rbx\n\t" + "movq %[tn], %%rax\n\t" + "mulq %[bd]\n\t" + "subq %[tmp], %%rax\n\t" + "sbbq %%rbx, %%rdx\n\t" // rdx:rax contains 128-bit-difference "numerator*b.denominator - b.numerator*denominator" + "setnsb %%bh\n\t" // bh=1 if difference is non-negative, bh=0 otherwise + "orq %%rdx, %%rax\n\t" + "setnzb %%bl\n\t" // bl=1 if difference if non-zero, bl=0 if it is zero + "decb %%bh\n\t" // now bx=0x0000 if difference is zero, 0xff01 if it is negative, 0x0001 if it is positive (i.e., same sign as difference) + "shll $16, %%ebx\n\t" // ebx has same sign as difference + : "=&b"(result), [tmp] "=&r"(tmp), "=a"(dummy) + : "a"(denominator), [bn] "g"(b.numerator), [tn] "g"(numerator), [bd] "g"(b.denominator) + : "%rdx", "cc" ); + return result ? result ^ sign // if sign is +1, only bit 0 of result is inverted, which does not change the sign of result (and cannot result in zero) + // if sign is -1, all bits of result are inverted, which changes the sign of result (and again cannot result in zero) + : 0; + +#else + + return sign * Int128::mul(m_numerator, b.m_denominator).ucmp(Int128::mul(m_denominator, b.m_numerator)); + +#endif +} + +int btConvexHullInternal::Rational128::compare(const Rational128& b) const +{ + if (sign != b.sign) + { + return sign - b.sign; + } + else if (sign == 0) + { + return 0; + } + if (isInt64) + { + return -b.compare(sign * (int64_t) numerator.low); + } + + Int128 nbdLow, nbdHigh, dbnLow, dbnHigh; + DMul<Int128, uint64_t>::mul(numerator, b.denominator, nbdLow, nbdHigh); + DMul<Int128, uint64_t>::mul(denominator, b.numerator, dbnLow, dbnHigh); + + int cmp = nbdHigh.ucmp(dbnHigh); + if (cmp) + { + return cmp * sign; + } + return nbdLow.ucmp(dbnLow) * sign; +} + +int btConvexHullInternal::Rational128::compare(int64_t b) const +{ + if (isInt64) + { + int64_t a = sign * (int64_t) numerator.low; + return (a > b) ? 1 : (a < b) ? -1 : 0; + } + if (b > 0) + { + if (sign <= 0) + { + return -1; + } + } + else if (b < 0) + { + if (sign >= 0) + { + return 1; + } + b = -b; + } + else + { + return sign; + } + + return numerator.ucmp(denominator * b) * sign; +} + + +btConvexHullInternal::Edge* btConvexHullInternal::newEdgePair(Vertex* from, Vertex* to) +{ + btAssert(from && to); + Edge* e = edgePool.newObject(); + Edge* r = edgePool.newObject(); + e->reverse = r; + r->reverse = e; + e->copy = mergeStamp; + r->copy = mergeStamp; + e->target = to; + r->target = from; + e->face = NULL; + r->face = NULL; + usedEdgePairs++; + if (usedEdgePairs > maxUsedEdgePairs) + { + maxUsedEdgePairs = usedEdgePairs; + } + return e; +} + +bool btConvexHullInternal::mergeProjection(IntermediateHull& h0, IntermediateHull& h1, Vertex*& c0, Vertex*& c1) +{ + Vertex* v0 = h0.maxYx; + Vertex* v1 = h1.minYx; + if ((v0->point.x == v1->point.x) && (v0->point.y == v1->point.y)) + { + btAssert(v0->point.z < v1->point.z); + Vertex* v1p = v1->prev; + if (v1p == v1) + { + c0 = v0; + if (v1->edges) + { + btAssert(v1->edges->next == v1->edges); + v1 = v1->edges->target; + btAssert(v1->edges->next == v1->edges); + } + c1 = v1; + return false; + } + Vertex* v1n = v1->next; + v1p->next = v1n; + v1n->prev = v1p; + if (v1 == h1.minXy) + { + if ((v1n->point.x < v1p->point.x) || ((v1n->point.x == v1p->point.x) && (v1n->point.y < v1p->point.y))) + { + h1.minXy = v1n; + } + else + { + h1.minXy = v1p; + } + } + if (v1 == h1.maxXy) + { + if ((v1n->point.x > v1p->point.x) || ((v1n->point.x == v1p->point.x) && (v1n->point.y > v1p->point.y))) + { + h1.maxXy = v1n; + } + else + { + h1.maxXy = v1p; + } + } + } + + v0 = h0.maxXy; + v1 = h1.maxXy; + Vertex* v00 = NULL; + Vertex* v10 = NULL; + int32_t sign = 1; + + for (int side = 0; side <= 1; side++) + { + int32_t dx = (v1->point.x - v0->point.x) * sign; + if (dx > 0) + { + while (true) + { + int32_t dy = v1->point.y - v0->point.y; + + Vertex* w0 = side ? v0->next : v0->prev; + if (w0 != v0) + { + int32_t dx0 = (w0->point.x - v0->point.x) * sign; + int32_t dy0 = w0->point.y - v0->point.y; + if ((dy0 <= 0) && ((dx0 == 0) || ((dx0 < 0) && (dy0 * dx <= dy * dx0)))) + { + v0 = w0; + dx = (v1->point.x - v0->point.x) * sign; + continue; + } + } + + Vertex* w1 = side ? v1->next : v1->prev; + if (w1 != v1) + { + int32_t dx1 = (w1->point.x - v1->point.x) * sign; + int32_t dy1 = w1->point.y - v1->point.y; + int32_t dxn = (w1->point.x - v0->point.x) * sign; + if ((dxn > 0) && (dy1 < 0) && ((dx1 == 0) || ((dx1 < 0) && (dy1 * dx < dy * dx1)))) + { + v1 = w1; + dx = dxn; + continue; + } + } + + break; + } + } + else if (dx < 0) + { + while (true) + { + int32_t dy = v1->point.y - v0->point.y; + + Vertex* w1 = side ? v1->prev : v1->next; + if (w1 != v1) + { + int32_t dx1 = (w1->point.x - v1->point.x) * sign; + int32_t dy1 = w1->point.y - v1->point.y; + if ((dy1 >= 0) && ((dx1 == 0) || ((dx1 < 0) && (dy1 * dx <= dy * dx1)))) + { + v1 = w1; + dx = (v1->point.x - v0->point.x) * sign; + continue; + } + } + + Vertex* w0 = side ? v0->prev : v0->next; + if (w0 != v0) + { + int32_t dx0 = (w0->point.x - v0->point.x) * sign; + int32_t dy0 = w0->point.y - v0->point.y; + int32_t dxn = (v1->point.x - w0->point.x) * sign; + if ((dxn < 0) && (dy0 > 0) && ((dx0 == 0) || ((dx0 < 0) && (dy0 * dx < dy * dx0)))) + { + v0 = w0; + dx = dxn; + continue; + } + } + + break; + } + } + else + { + int32_t x = v0->point.x; + int32_t y0 = v0->point.y; + Vertex* w0 = v0; + Vertex* t; + while (((t = side ? w0->next : w0->prev) != v0) && (t->point.x == x) && (t->point.y <= y0)) + { + w0 = t; + y0 = t->point.y; + } + v0 = w0; + + int32_t y1 = v1->point.y; + Vertex* w1 = v1; + while (((t = side ? w1->prev : w1->next) != v1) && (t->point.x == x) && (t->point.y >= y1)) + { + w1 = t; + y1 = t->point.y; + } + v1 = w1; + } + + if (side == 0) + { + v00 = v0; + v10 = v1; + + v0 = h0.minXy; + v1 = h1.minXy; + sign = -1; + } + } + + v0->prev = v1; + v1->next = v0; + + v00->next = v10; + v10->prev = v00; + + if (h1.minXy->point.x < h0.minXy->point.x) + { + h0.minXy = h1.minXy; + } + if (h1.maxXy->point.x >= h0.maxXy->point.x) + { + h0.maxXy = h1.maxXy; + } + + h0.maxYx = h1.maxYx; + + c0 = v00; + c1 = v10; + + return true; +} + +void btConvexHullInternal::computeInternal(int start, int end, IntermediateHull& result) +{ + int n = end - start; + switch (n) + { + case 0: + result.minXy = NULL; + result.maxXy = NULL; + result.minYx = NULL; + result.maxYx = NULL; + return; + case 2: + { + Vertex* v = originalVertices[start]; + Vertex* w = v + 1; + if (v->point != w->point) + { + int32_t dx = v->point.x - w->point.x; + int32_t dy = v->point.y - w->point.y; + + if ((dx == 0) && (dy == 0)) + { + if (v->point.z > w->point.z) + { + Vertex* t = w; + w = v; + v = t; + } + btAssert(v->point.z < w->point.z); + v->next = v; + v->prev = v; + result.minXy = v; + result.maxXy = v; + result.minYx = v; + result.maxYx = v; + } + else + { + v->next = w; + v->prev = w; + w->next = v; + w->prev = v; + + if ((dx < 0) || ((dx == 0) && (dy < 0))) + { + result.minXy = v; + result.maxXy = w; + } + else + { + result.minXy = w; + result.maxXy = v; + } + + if ((dy < 0) || ((dy == 0) && (dx < 0))) + { + result.minYx = v; + result.maxYx = w; + } + else + { + result.minYx = w; + result.maxYx = v; + } + } + + Edge* e = newEdgePair(v, w); + e->link(e); + v->edges = e; + + e = e->reverse; + e->link(e); + w->edges = e; + + return; + } + } + // lint -fallthrough + case 1: + { + Vertex* v = originalVertices[start]; + v->edges = NULL; + v->next = v; + v->prev = v; + + result.minXy = v; + result.maxXy = v; + result.minYx = v; + result.maxYx = v; + + return; + } + } + + int split0 = start + n / 2; + Point32 p = originalVertices[split0-1]->point; + int split1 = split0; + while ((split1 < end) && (originalVertices[split1]->point == p)) + { + split1++; + } + computeInternal(start, split0, result); + IntermediateHull hull1; + computeInternal(split1, end, hull1); +#ifdef DEBUG_CONVEX_HULL + printf("\n\nMerge\n"); + result.print(); + hull1.print(); +#endif + merge(result, hull1); +#ifdef DEBUG_CONVEX_HULL + printf("\n Result\n"); + result.print(); +#endif +} + +#ifdef DEBUG_CONVEX_HULL +void btConvexHullInternal::IntermediateHull::print() +{ + printf(" Hull\n"); + for (Vertex* v = minXy; v; ) + { + printf(" "); + v->print(); + if (v == maxXy) + { + printf(" maxXy"); + } + if (v == minYx) + { + printf(" minYx"); + } + if (v == maxYx) + { + printf(" maxYx"); + } + if (v->next->prev != v) + { + printf(" Inconsistency"); + } + printf("\n"); + v = v->next; + if (v == minXy) + { + break; + } + } + if (minXy) + { + minXy->copy = (minXy->copy == -1) ? -2 : -1; + minXy->printGraph(); + } +} + +void btConvexHullInternal::Vertex::printGraph() +{ + print(); + printf("\nEdges\n"); + Edge* e = edges; + if (e) + { + do + { + e->print(); + printf("\n"); + e = e->next; + } while (e != edges); + do + { + Vertex* v = e->target; + if (v->copy != copy) + { + v->copy = copy; + v->printGraph(); + } + e = e->next; + } while (e != edges); + } +} +#endif + +btConvexHullInternal::Orientation btConvexHullInternal::getOrientation(const Edge* prev, const Edge* next, const Point32& s, const Point32& t) +{ + btAssert(prev->reverse->target == next->reverse->target); + if (prev->next == next) + { + if (prev->prev == next) + { + Point64 n = t.cross(s); + Point64 m = (*prev->target - *next->reverse->target).cross(*next->target - *next->reverse->target); + btAssert(!m.isZero()); + int64_t dot = n.dot(m); + btAssert(dot != 0); + return (dot > 0) ? COUNTER_CLOCKWISE : CLOCKWISE; + } + return COUNTER_CLOCKWISE; + } + else if (prev->prev == next) + { + return CLOCKWISE; + } + else + { + return NONE; + } +} + +btConvexHullInternal::Edge* btConvexHullInternal::findMaxAngle(bool ccw, const Vertex* start, const Point32& s, const Point64& rxs, const Point64& sxrxs, Rational64& minCot) +{ + Edge* minEdge = NULL; + +#ifdef DEBUG_CONVEX_HULL + printf("find max edge for %d\n", start->point.index); +#endif + Edge* e = start->edges; + if (e) + { + do + { + if (e->copy > mergeStamp) + { + Point32 t = *e->target - *start; + Rational64 cot(t.dot(sxrxs), t.dot(rxs)); +#ifdef DEBUG_CONVEX_HULL + printf(" Angle is %f (%d) for ", (float) btAtan(cot.toScalar()), (int) cot.isNaN()); + e->print(); +#endif + if (cot.isNaN()) + { + btAssert(ccw ? (t.dot(s) < 0) : (t.dot(s) > 0)); + } + else + { + int cmp; + if (minEdge == NULL) + { + minCot = cot; + minEdge = e; + } + else if ((cmp = cot.compare(minCot)) < 0) + { + minCot = cot; + minEdge = e; + } + else if ((cmp == 0) && (ccw == (getOrientation(minEdge, e, s, t) == COUNTER_CLOCKWISE))) + { + minEdge = e; + } + } +#ifdef DEBUG_CONVEX_HULL + printf("\n"); +#endif + } + e = e->next; + } while (e != start->edges); + } + return minEdge; +} + +void btConvexHullInternal::findEdgeForCoplanarFaces(Vertex* c0, Vertex* c1, Edge*& e0, Edge*& e1, Vertex* stop0, Vertex* stop1) +{ + Edge* start0 = e0; + Edge* start1 = e1; + Point32 et0 = start0 ? start0->target->point : c0->point; + Point32 et1 = start1 ? start1->target->point : c1->point; + Point32 s = c1->point - c0->point; + Point64 normal = ((start0 ? start0 : start1)->target->point - c0->point).cross(s); + int64_t dist = c0->point.dot(normal); + btAssert(!start1 || (start1->target->point.dot(normal) == dist)); + Point64 perp = s.cross(normal); + btAssert(!perp.isZero()); + +#ifdef DEBUG_CONVEX_HULL + printf(" Advancing %d %d (%p %p, %d %d)\n", c0->point.index, c1->point.index, start0, start1, start0 ? start0->target->point.index : -1, start1 ? start1->target->point.index : -1); +#endif + + int64_t maxDot0 = et0.dot(perp); + if (e0) + { + while (e0->target != stop0) + { + Edge* e = e0->reverse->prev; + if (e->target->point.dot(normal) < dist) + { + break; + } + btAssert(e->target->point.dot(normal) == dist); + if (e->copy == mergeStamp) + { + break; + } + int64_t dot = e->target->point.dot(perp); + if (dot <= maxDot0) + { + break; + } + maxDot0 = dot; + e0 = e; + et0 = e->target->point; + } + } + + int64_t maxDot1 = et1.dot(perp); + if (e1) + { + while (e1->target != stop1) + { + Edge* e = e1->reverse->next; + if (e->target->point.dot(normal) < dist) + { + break; + } + btAssert(e->target->point.dot(normal) == dist); + if (e->copy == mergeStamp) + { + break; + } + int64_t dot = e->target->point.dot(perp); + if (dot <= maxDot1) + { + break; + } + maxDot1 = dot; + e1 = e; + et1 = e->target->point; + } + } + +#ifdef DEBUG_CONVEX_HULL + printf(" Starting at %d %d\n", et0.index, et1.index); +#endif + + int64_t dx = maxDot1 - maxDot0; + if (dx > 0) + { + while (true) + { + int64_t dy = (et1 - et0).dot(s); + + if (e0 && (e0->target != stop0)) + { + Edge* f0 = e0->next->reverse; + if (f0->copy > mergeStamp) + { + int64_t dx0 = (f0->target->point - et0).dot(perp); + int64_t dy0 = (f0->target->point - et0).dot(s); + if ((dx0 == 0) ? (dy0 < 0) : ((dx0 < 0) && (Rational64(dy0, dx0).compare(Rational64(dy, dx)) >= 0))) + { + et0 = f0->target->point; + dx = (et1 - et0).dot(perp); + e0 = (e0 == start0) ? NULL : f0; + continue; + } + } + } + + if (e1 && (e1->target != stop1)) + { + Edge* f1 = e1->reverse->next; + if (f1->copy > mergeStamp) + { + Point32 d1 = f1->target->point - et1; + if (d1.dot(normal) == 0) + { + int64_t dx1 = d1.dot(perp); + int64_t dy1 = d1.dot(s); + int64_t dxn = (f1->target->point - et0).dot(perp); + if ((dxn > 0) && ((dx1 == 0) ? (dy1 < 0) : ((dx1 < 0) && (Rational64(dy1, dx1).compare(Rational64(dy, dx)) > 0)))) + { + e1 = f1; + et1 = e1->target->point; + dx = dxn; + continue; + } + } + else + { + btAssert((e1 == start1) && (d1.dot(normal) < 0)); + } + } + } + + break; + } + } + else if (dx < 0) + { + while (true) + { + int64_t dy = (et1 - et0).dot(s); + + if (e1 && (e1->target != stop1)) + { + Edge* f1 = e1->prev->reverse; + if (f1->copy > mergeStamp) + { + int64_t dx1 = (f1->target->point - et1).dot(perp); + int64_t dy1 = (f1->target->point - et1).dot(s); + if ((dx1 == 0) ? (dy1 > 0) : ((dx1 < 0) && (Rational64(dy1, dx1).compare(Rational64(dy, dx)) <= 0))) + { + et1 = f1->target->point; + dx = (et1 - et0).dot(perp); + e1 = (e1 == start1) ? NULL : f1; + continue; + } + } + } + + if (e0 && (e0->target != stop0)) + { + Edge* f0 = e0->reverse->prev; + if (f0->copy > mergeStamp) + { + Point32 d0 = f0->target->point - et0; + if (d0.dot(normal) == 0) + { + int64_t dx0 = d0.dot(perp); + int64_t dy0 = d0.dot(s); + int64_t dxn = (et1 - f0->target->point).dot(perp); + if ((dxn < 0) && ((dx0 == 0) ? (dy0 > 0) : ((dx0 < 0) && (Rational64(dy0, dx0).compare(Rational64(dy, dx)) < 0)))) + { + e0 = f0; + et0 = e0->target->point; + dx = dxn; + continue; + } + } + else + { + btAssert((e0 == start0) && (d0.dot(normal) < 0)); + } + } + } + + break; + } + } +#ifdef DEBUG_CONVEX_HULL + printf(" Advanced edges to %d %d\n", et0.index, et1.index); +#endif +} + + +void btConvexHullInternal::merge(IntermediateHull& h0, IntermediateHull& h1) +{ + if (!h1.maxXy) + { + return; + } + if (!h0.maxXy) + { + h0 = h1; + return; + } + + mergeStamp--; + + Vertex* c0 = NULL; + Edge* toPrev0 = NULL; + Edge* firstNew0 = NULL; + Edge* pendingHead0 = NULL; + Edge* pendingTail0 = NULL; + Vertex* c1 = NULL; + Edge* toPrev1 = NULL; + Edge* firstNew1 = NULL; + Edge* pendingHead1 = NULL; + Edge* pendingTail1 = NULL; + Point32 prevPoint; + + if (mergeProjection(h0, h1, c0, c1)) + { + Point32 s = *c1 - *c0; + Point64 normal = Point32(0, 0, -1).cross(s); + Point64 t = s.cross(normal); + btAssert(!t.isZero()); + + Edge* e = c0->edges; + Edge* start0 = NULL; + if (e) + { + do + { + int64_t dot = (*e->target - *c0).dot(normal); + btAssert(dot <= 0); + if ((dot == 0) && ((*e->target - *c0).dot(t) > 0)) + { + if (!start0 || (getOrientation(start0, e, s, Point32(0, 0, -1)) == CLOCKWISE)) + { + start0 = e; + } + } + e = e->next; + } while (e != c0->edges); + } + + e = c1->edges; + Edge* start1 = NULL; + if (e) + { + do + { + int64_t dot = (*e->target - *c1).dot(normal); + btAssert(dot <= 0); + if ((dot == 0) && ((*e->target - *c1).dot(t) > 0)) + { + if (!start1 || (getOrientation(start1, e, s, Point32(0, 0, -1)) == COUNTER_CLOCKWISE)) + { + start1 = e; + } + } + e = e->next; + } while (e != c1->edges); + } + + if (start0 || start1) + { + findEdgeForCoplanarFaces(c0, c1, start0, start1, NULL, NULL); + if (start0) + { + c0 = start0->target; + } + if (start1) + { + c1 = start1->target; + } + } + + prevPoint = c1->point; + prevPoint.z++; + } + else + { + prevPoint = c1->point; + prevPoint.x++; + } + + Vertex* first0 = c0; + Vertex* first1 = c1; + bool firstRun = true; + + while (true) + { + Point32 s = *c1 - *c0; + Point32 r = prevPoint - c0->point; + Point64 rxs = r.cross(s); + Point64 sxrxs = s.cross(rxs); + +#ifdef DEBUG_CONVEX_HULL + printf("\n Checking %d %d\n", c0->point.index, c1->point.index); +#endif + Rational64 minCot0(0, 0); + Edge* min0 = findMaxAngle(false, c0, s, rxs, sxrxs, minCot0); + Rational64 minCot1(0, 0); + Edge* min1 = findMaxAngle(true, c1, s, rxs, sxrxs, minCot1); + if (!min0 && !min1) + { + Edge* e = newEdgePair(c0, c1); + e->link(e); + c0->edges = e; + + e = e->reverse; + e->link(e); + c1->edges = e; + return; + } + else + { + int cmp = !min0 ? 1 : !min1 ? -1 : minCot0.compare(minCot1); +#ifdef DEBUG_CONVEX_HULL + printf(" -> Result %d\n", cmp); +#endif + if (firstRun || ((cmp >= 0) ? !minCot1.isNegativeInfinity() : !minCot0.isNegativeInfinity())) + { + Edge* e = newEdgePair(c0, c1); + if (pendingTail0) + { + pendingTail0->prev = e; + } + else + { + pendingHead0 = e; + } + e->next = pendingTail0; + pendingTail0 = e; + + e = e->reverse; + if (pendingTail1) + { + pendingTail1->next = e; + } + else + { + pendingHead1 = e; + } + e->prev = pendingTail1; + pendingTail1 = e; + } + + Edge* e0 = min0; + Edge* e1 = min1; + +#ifdef DEBUG_CONVEX_HULL + printf(" Found min edges to %d %d\n", e0 ? e0->target->point.index : -1, e1 ? e1->target->point.index : -1); +#endif + + if (cmp == 0) + { + findEdgeForCoplanarFaces(c0, c1, e0, e1, NULL, NULL); + } + + if ((cmp >= 0) && e1) + { + if (toPrev1) + { + for (Edge* e = toPrev1->next, *n = NULL; e != min1; e = n) + { + n = e->next; + removeEdgePair(e); + } + } + + if (pendingTail1) + { + if (toPrev1) + { + toPrev1->link(pendingHead1); + } + else + { + min1->prev->link(pendingHead1); + firstNew1 = pendingHead1; + } + pendingTail1->link(min1); + pendingHead1 = NULL; + pendingTail1 = NULL; + } + else if (!toPrev1) + { + firstNew1 = min1; + } + + prevPoint = c1->point; + c1 = e1->target; + toPrev1 = e1->reverse; + } + + if ((cmp <= 0) && e0) + { + if (toPrev0) + { + for (Edge* e = toPrev0->prev, *n = NULL; e != min0; e = n) + { + n = e->prev; + removeEdgePair(e); + } + } + + if (pendingTail0) + { + if (toPrev0) + { + pendingHead0->link(toPrev0); + } + else + { + pendingHead0->link(min0->next); + firstNew0 = pendingHead0; + } + min0->link(pendingTail0); + pendingHead0 = NULL; + pendingTail0 = NULL; + } + else if (!toPrev0) + { + firstNew0 = min0; + } + + prevPoint = c0->point; + c0 = e0->target; + toPrev0 = e0->reverse; + } + } + + if ((c0 == first0) && (c1 == first1)) + { + if (toPrev0 == NULL) + { + pendingHead0->link(pendingTail0); + c0->edges = pendingTail0; + } + else + { + for (Edge* e = toPrev0->prev, *n = NULL; e != firstNew0; e = n) + { + n = e->prev; + removeEdgePair(e); + } + if (pendingTail0) + { + pendingHead0->link(toPrev0); + firstNew0->link(pendingTail0); + } + } + + if (toPrev1 == NULL) + { + pendingTail1->link(pendingHead1); + c1->edges = pendingTail1; + } + else + { + for (Edge* e = toPrev1->next, *n = NULL; e != firstNew1; e = n) + { + n = e->next; + removeEdgePair(e); + } + if (pendingTail1) + { + toPrev1->link(pendingHead1); + pendingTail1->link(firstNew1); + } + } + + return; + } + + firstRun = false; + } +} + + +static bool pointCmp(const btConvexHullInternal::Point32& p, const btConvexHullInternal::Point32& q) +{ + return (p.y < q.y) || ((p.y == q.y) && ((p.x < q.x) || ((p.x == q.x) && (p.z < q.z)))); +} + +void btConvexHullInternal::compute(const void* coords, bool doubleCoords, int stride, int count) +{ + btVector3 min(btScalar(1e30), btScalar(1e30), btScalar(1e30)), max(btScalar(-1e30), btScalar(-1e30), btScalar(-1e30)); + const char* ptr = (const char*) coords; + if (doubleCoords) + { + for (int i = 0; i < count; i++) + { + const double* v = (const double*) ptr; + btVector3 p((btScalar) v[0], (btScalar) v[1], (btScalar) v[2]); + ptr += stride; + min.setMin(p); + max.setMax(p); + } + } + else + { + for (int i = 0; i < count; i++) + { + const float* v = (const float*) ptr; + btVector3 p(v[0], v[1], v[2]); + ptr += stride; + min.setMin(p); + max.setMax(p); + } + } + + btVector3 s = max - min; + maxAxis = s.maxAxis(); + minAxis = s.minAxis(); + if (minAxis == maxAxis) + { + minAxis = (maxAxis + 1) % 3; + } + medAxis = 3 - maxAxis - minAxis; + + s /= btScalar(10216); + if (((medAxis + 1) % 3) != maxAxis) + { + s *= -1; + } + scaling = s; + + if (s[0] != 0) + { + s[0] = btScalar(1) / s[0]; + } + if (s[1] != 0) + { + s[1] = btScalar(1) / s[1]; + } + if (s[2] != 0) + { + s[2] = btScalar(1) / s[2]; + } + + center = (min + max) * btScalar(0.5); + + btAlignedObjectArray<Point32> points; + points.resize(count); + ptr = (const char*) coords; + if (doubleCoords) + { + for (int i = 0; i < count; i++) + { + const double* v = (const double*) ptr; + btVector3 p((btScalar) v[0], (btScalar) v[1], (btScalar) v[2]); + ptr += stride; + p = (p - center) * s; + points[i].x = (int32_t) p[medAxis]; + points[i].y = (int32_t) p[maxAxis]; + points[i].z = (int32_t) p[minAxis]; + points[i].index = i; + } + } + else + { + for (int i = 0; i < count; i++) + { + const float* v = (const float*) ptr; + btVector3 p(v[0], v[1], v[2]); + ptr += stride; + p = (p - center) * s; + points[i].x = (int32_t) p[medAxis]; + points[i].y = (int32_t) p[maxAxis]; + points[i].z = (int32_t) p[minAxis]; + points[i].index = i; + } + } + points.quickSort(pointCmp); + + vertexPool.reset(); + vertexPool.setArraySize(count); + originalVertices.resize(count); + for (int i = 0; i < count; i++) + { + Vertex* v = vertexPool.newObject(); + v->edges = NULL; + v->point = points[i]; + v->copy = -1; + originalVertices[i] = v; + } + + points.clear(); + + edgePool.reset(); + edgePool.setArraySize(6 * count); + + usedEdgePairs = 0; + maxUsedEdgePairs = 0; + + mergeStamp = -3; + + IntermediateHull hull; + computeInternal(0, count, hull); + vertexList = hull.minXy; +#ifdef DEBUG_CONVEX_HULL + printf("max. edges %d (3v = %d)", maxUsedEdgePairs, 3 * count); +#endif +} + +btVector3 btConvexHullInternal::toBtVector(const Point32& v) +{ + btVector3 p; + p[medAxis] = btScalar(v.x); + p[maxAxis] = btScalar(v.y); + p[minAxis] = btScalar(v.z); + return p * scaling; +} + +btVector3 btConvexHullInternal::getBtNormal(Face* face) +{ + return toBtVector(face->dir0).cross(toBtVector(face->dir1)).normalized(); +} + +btVector3 btConvexHullInternal::getCoordinates(const Vertex* v) +{ + btVector3 p; + p[medAxis] = v->xvalue(); + p[maxAxis] = v->yvalue(); + p[minAxis] = v->zvalue(); + return p * scaling + center; +} + +btScalar btConvexHullInternal::shrink(btScalar amount, btScalar clampAmount) +{ + if (!vertexList) + { + return 0; + } + int stamp = --mergeStamp; + btAlignedObjectArray<Vertex*> stack; + vertexList->copy = stamp; + stack.push_back(vertexList); + btAlignedObjectArray<Face*> faces; + + Point32 ref = vertexList->point; + Int128 hullCenterX(0, 0); + Int128 hullCenterY(0, 0); + Int128 hullCenterZ(0, 0); + Int128 volume(0, 0); + + while (stack.size() > 0) + { + Vertex* v = stack[stack.size() - 1]; + stack.pop_back(); + Edge* e = v->edges; + if (e) + { + do + { + if (e->target->copy != stamp) + { + e->target->copy = stamp; + stack.push_back(e->target); + } + if (e->copy != stamp) + { + Face* face = facePool.newObject(); + face->init(e->target, e->reverse->prev->target, v); + faces.push_back(face); + Edge* f = e; + + Vertex* a = NULL; + Vertex* b = NULL; + do + { + if (a && b) + { + int64_t vol = (v->point - ref).dot((a->point - ref).cross(b->point - ref)); + btAssert(vol >= 0); + Point32 c = v->point + a->point + b->point + ref; + hullCenterX += vol * c.x; + hullCenterY += vol * c.y; + hullCenterZ += vol * c.z; + volume += vol; + } + + btAssert(f->copy != stamp); + f->copy = stamp; + f->face = face; + + a = b; + b = f->target; + + f = f->reverse->prev; + } while (f != e); + } + e = e->next; + } while (e != v->edges); + } + } + + if (volume.getSign() <= 0) + { + return 0; + } + + btVector3 hullCenter; + hullCenter[medAxis] = hullCenterX.toScalar(); + hullCenter[maxAxis] = hullCenterY.toScalar(); + hullCenter[minAxis] = hullCenterZ.toScalar(); + hullCenter /= 4 * volume.toScalar(); + hullCenter *= scaling; + + int faceCount = faces.size(); + + if (clampAmount > 0) + { + btScalar minDist = SIMD_INFINITY; + for (int i = 0; i < faceCount; i++) + { + btVector3 normal = getBtNormal(faces[i]); + btScalar dist = normal.dot(toBtVector(faces[i]->origin) - hullCenter); + if (dist < minDist) + { + minDist = dist; + } + } + + if (minDist <= 0) + { + return 0; + } + + amount = btMin(amount, minDist * clampAmount); + } + + unsigned int seed = 243703; + for (int i = 0; i < faceCount; i++, seed = 1664525 * seed + 1013904223) + { + btSwap(faces[i], faces[seed % faceCount]); + } + + for (int i = 0; i < faceCount; i++) + { + if (!shiftFace(faces[i], amount, stack)) + { + return -amount; + } + } + + return amount; +} + +bool btConvexHullInternal::shiftFace(Face* face, btScalar amount, btAlignedObjectArray<Vertex*> stack) +{ + btVector3 origShift = getBtNormal(face) * -amount; + if (scaling[0] != 0) + { + origShift[0] /= scaling[0]; + } + if (scaling[1] != 0) + { + origShift[1] /= scaling[1]; + } + if (scaling[2] != 0) + { + origShift[2] /= scaling[2]; + } + Point32 shift((int32_t) origShift[medAxis], (int32_t) origShift[maxAxis], (int32_t) origShift[minAxis]); + if (shift.isZero()) + { + return true; + } + Point64 normal = face->getNormal(); +#ifdef DEBUG_CONVEX_HULL + printf("\nShrinking face (%d %d %d) (%d %d %d) (%d %d %d) by (%d %d %d)\n", + face->origin.x, face->origin.y, face->origin.z, face->dir0.x, face->dir0.y, face->dir0.z, face->dir1.x, face->dir1.y, face->dir1.z, shift.x, shift.y, shift.z); +#endif + int64_t origDot = face->origin.dot(normal); + Point32 shiftedOrigin = face->origin + shift; + int64_t shiftedDot = shiftedOrigin.dot(normal); + btAssert(shiftedDot <= origDot); + if (shiftedDot >= origDot) + { + return false; + } + + Edge* intersection = NULL; + + Edge* startEdge = face->nearbyVertex->edges; +#ifdef DEBUG_CONVEX_HULL + printf("Start edge is "); + startEdge->print(); + printf(", normal is (%lld %lld %lld), shifted dot is %lld\n", normal.x, normal.y, normal.z, shiftedDot); +#endif + Rational128 optDot = face->nearbyVertex->dot(normal); + int cmp = optDot.compare(shiftedDot); +#ifdef SHOW_ITERATIONS + int n = 0; +#endif + if (cmp >= 0) + { + Edge* e = startEdge; + do + { +#ifdef SHOW_ITERATIONS + n++; +#endif + Rational128 dot = e->target->dot(normal); + btAssert(dot.compare(origDot) <= 0); +#ifdef DEBUG_CONVEX_HULL + printf("Moving downwards, edge is "); + e->print(); + printf(", dot is %f (%f %lld)\n", (float) dot.toScalar(), (float) optDot.toScalar(), shiftedDot); +#endif + if (dot.compare(optDot) < 0) + { + int c = dot.compare(shiftedDot); + optDot = dot; + e = e->reverse; + startEdge = e; + if (c < 0) + { + intersection = e; + break; + } + cmp = c; + } + e = e->prev; + } while (e != startEdge); + + if (!intersection) + { + return false; + } + } + else + { + Edge* e = startEdge; + do + { +#ifdef SHOW_ITERATIONS + n++; +#endif + Rational128 dot = e->target->dot(normal); + btAssert(dot.compare(origDot) <= 0); +#ifdef DEBUG_CONVEX_HULL + printf("Moving upwards, edge is "); + e->print(); + printf(", dot is %f (%f %lld)\n", (float) dot.toScalar(), (float) optDot.toScalar(), shiftedDot); +#endif + if (dot.compare(optDot) > 0) + { + cmp = dot.compare(shiftedDot); + if (cmp >= 0) + { + intersection = e; + break; + } + optDot = dot; + e = e->reverse; + startEdge = e; + } + e = e->prev; + } while (e != startEdge); + + if (!intersection) + { + return true; + } + } + +#ifdef SHOW_ITERATIONS + printf("Needed %d iterations to find initial intersection\n", n); +#endif + + if (cmp == 0) + { + Edge* e = intersection->reverse->next; +#ifdef SHOW_ITERATIONS + n = 0; +#endif + while (e->target->dot(normal).compare(shiftedDot) <= 0) + { +#ifdef SHOW_ITERATIONS + n++; +#endif + e = e->next; + if (e == intersection->reverse) + { + return true; + } +#ifdef DEBUG_CONVEX_HULL + printf("Checking for outwards edge, current edge is "); + e->print(); + printf("\n"); +#endif + } +#ifdef SHOW_ITERATIONS + printf("Needed %d iterations to check for complete containment\n", n); +#endif + } + + Edge* firstIntersection = NULL; + Edge* faceEdge = NULL; + Edge* firstFaceEdge = NULL; + +#ifdef SHOW_ITERATIONS + int m = 0; +#endif + while (true) + { +#ifdef SHOW_ITERATIONS + m++; +#endif +#ifdef DEBUG_CONVEX_HULL + printf("Intersecting edge is "); + intersection->print(); + printf("\n"); +#endif + if (cmp == 0) + { + Edge* e = intersection->reverse->next; + startEdge = e; +#ifdef SHOW_ITERATIONS + n = 0; +#endif + while (true) + { +#ifdef SHOW_ITERATIONS + n++; +#endif + if (e->target->dot(normal).compare(shiftedDot) >= 0) + { + break; + } + intersection = e->reverse; + e = e->next; + if (e == startEdge) + { + return true; + } + } +#ifdef SHOW_ITERATIONS + printf("Needed %d iterations to advance intersection\n", n); +#endif + } + +#ifdef DEBUG_CONVEX_HULL + printf("Advanced intersecting edge to "); + intersection->print(); + printf(", cmp = %d\n", cmp); +#endif + + if (!firstIntersection) + { + firstIntersection = intersection; + } + else if (intersection == firstIntersection) + { + break; + } + + int prevCmp = cmp; + Edge* prevIntersection = intersection; + Edge* prevFaceEdge = faceEdge; + + Edge* e = intersection->reverse; +#ifdef SHOW_ITERATIONS + n = 0; +#endif + while (true) + { +#ifdef SHOW_ITERATIONS + n++; +#endif + e = e->reverse->prev; + btAssert(e != intersection->reverse); + cmp = e->target->dot(normal).compare(shiftedDot); +#ifdef DEBUG_CONVEX_HULL + printf("Testing edge "); + e->print(); + printf(" -> cmp = %d\n", cmp); +#endif + if (cmp >= 0) + { + intersection = e; + break; + } + } +#ifdef SHOW_ITERATIONS + printf("Needed %d iterations to find other intersection of face\n", n); +#endif + + if (cmp > 0) + { + Vertex* removed = intersection->target; + e = intersection->reverse; + if (e->prev == e) + { + removed->edges = NULL; + } + else + { + removed->edges = e->prev; + e->prev->link(e->next); + e->link(e); + } +#ifdef DEBUG_CONVEX_HULL + printf("1: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z); +#endif + + Point64 n0 = intersection->face->getNormal(); + Point64 n1 = intersection->reverse->face->getNormal(); + int64_t m00 = face->dir0.dot(n0); + int64_t m01 = face->dir1.dot(n0); + int64_t m10 = face->dir0.dot(n1); + int64_t m11 = face->dir1.dot(n1); + int64_t r0 = (intersection->face->origin - shiftedOrigin).dot(n0); + int64_t r1 = (intersection->reverse->face->origin - shiftedOrigin).dot(n1); + Int128 det = Int128::mul(m00, m11) - Int128::mul(m01, m10); + btAssert(det.getSign() != 0); + Vertex* v = vertexPool.newObject(); + v->point.index = -1; + v->copy = -1; + v->point128 = PointR128(Int128::mul(face->dir0.x * r0, m11) - Int128::mul(face->dir0.x * r1, m01) + + Int128::mul(face->dir1.x * r1, m00) - Int128::mul(face->dir1.x * r0, m10) + det * shiftedOrigin.x, + Int128::mul(face->dir0.y * r0, m11) - Int128::mul(face->dir0.y * r1, m01) + + Int128::mul(face->dir1.y * r1, m00) - Int128::mul(face->dir1.y * r0, m10) + det * shiftedOrigin.y, + Int128::mul(face->dir0.z * r0, m11) - Int128::mul(face->dir0.z * r1, m01) + + Int128::mul(face->dir1.z * r1, m00) - Int128::mul(face->dir1.z * r0, m10) + det * shiftedOrigin.z, + det); + v->point.x = (int32_t) v->point128.xvalue(); + v->point.y = (int32_t) v->point128.yvalue(); + v->point.z = (int32_t) v->point128.zvalue(); + intersection->target = v; + v->edges = e; + + stack.push_back(v); + stack.push_back(removed); + stack.push_back(NULL); + } + + if (cmp || prevCmp || (prevIntersection->reverse->next->target != intersection->target)) + { + faceEdge = newEdgePair(prevIntersection->target, intersection->target); + if (prevCmp == 0) + { + faceEdge->link(prevIntersection->reverse->next); + } + if ((prevCmp == 0) || prevFaceEdge) + { + prevIntersection->reverse->link(faceEdge); + } + if (cmp == 0) + { + intersection->reverse->prev->link(faceEdge->reverse); + } + faceEdge->reverse->link(intersection->reverse); + } + else + { + faceEdge = prevIntersection->reverse->next; + } + + if (prevFaceEdge) + { + if (prevCmp > 0) + { + faceEdge->link(prevFaceEdge->reverse); + } + else if (faceEdge != prevFaceEdge->reverse) + { + stack.push_back(prevFaceEdge->target); + while (faceEdge->next != prevFaceEdge->reverse) + { + Vertex* removed = faceEdge->next->target; + removeEdgePair(faceEdge->next); + stack.push_back(removed); +#ifdef DEBUG_CONVEX_HULL + printf("2: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z); +#endif + } + stack.push_back(NULL); + } + } + faceEdge->face = face; + faceEdge->reverse->face = intersection->face; + + if (!firstFaceEdge) + { + firstFaceEdge = faceEdge; + } + } +#ifdef SHOW_ITERATIONS + printf("Needed %d iterations to process all intersections\n", m); +#endif + + if (cmp > 0) + { + firstFaceEdge->reverse->target = faceEdge->target; + firstIntersection->reverse->link(firstFaceEdge); + firstFaceEdge->link(faceEdge->reverse); + } + else if (firstFaceEdge != faceEdge->reverse) + { + stack.push_back(faceEdge->target); + while (firstFaceEdge->next != faceEdge->reverse) + { + Vertex* removed = firstFaceEdge->next->target; + removeEdgePair(firstFaceEdge->next); + stack.push_back(removed); +#ifdef DEBUG_CONVEX_HULL + printf("3: Removed part contains (%d %d %d)\n", removed->point.x, removed->point.y, removed->point.z); +#endif + } + stack.push_back(NULL); + } + + btAssert(stack.size() > 0); + vertexList = stack[0]; + +#ifdef DEBUG_CONVEX_HULL + printf("Removing part\n"); +#endif +#ifdef SHOW_ITERATIONS + n = 0; +#endif + int pos = 0; + while (pos < stack.size()) + { + int end = stack.size(); + while (pos < end) + { + Vertex* kept = stack[pos++]; +#ifdef DEBUG_CONVEX_HULL + kept->print(); +#endif + bool deeper = false; + Vertex* removed; + while ((removed = stack[pos++]) != NULL) + { +#ifdef SHOW_ITERATIONS + n++; +#endif + kept->receiveNearbyFaces(removed); + while (removed->edges) + { + if (!deeper) + { + deeper = true; + stack.push_back(kept); + } + stack.push_back(removed->edges->target); + removeEdgePair(removed->edges); + } + } + if (deeper) + { + stack.push_back(NULL); + } + } + } +#ifdef SHOW_ITERATIONS + printf("Needed %d iterations to remove part\n", n); +#endif + + stack.resize(0); + face->origin = shiftedOrigin; + + return true; +} + + +static int getVertexCopy(btConvexHullInternal::Vertex* vertex, btAlignedObjectArray<btConvexHullInternal::Vertex*>& vertices) +{ + int index = vertex->copy; + if (index < 0) + { + index = vertices.size(); + vertex->copy = index; + vertices.push_back(vertex); +#ifdef DEBUG_CONVEX_HULL + printf("Vertex %d gets index *%d\n", vertex->point.index, index); +#endif + } + return index; +} + +btScalar btConvexHullComputer::compute(const void* coords, bool doubleCoords, int stride, int count, btScalar shrink, btScalar shrinkClamp) +{ + if (count <= 0) + { + vertices.clear(); + edges.clear(); + faces.clear(); + return 0; + } + + btConvexHullInternal hull; + hull.compute(coords, doubleCoords, stride, count); + + btScalar shift = 0; + if ((shrink > 0) && ((shift = hull.shrink(shrink, shrinkClamp)) < 0)) + { + vertices.clear(); + edges.clear(); + faces.clear(); + return shift; + } + + vertices.resize(0); + edges.resize(0); + faces.resize(0); + + btAlignedObjectArray<btConvexHullInternal::Vertex*> oldVertices; + getVertexCopy(hull.vertexList, oldVertices); + int copied = 0; + while (copied < oldVertices.size()) + { + btConvexHullInternal::Vertex* v = oldVertices[copied]; + vertices.push_back(hull.getCoordinates(v)); + btConvexHullInternal::Edge* firstEdge = v->edges; + if (firstEdge) + { + int firstCopy = -1; + int prevCopy = -1; + btConvexHullInternal::Edge* e = firstEdge; + do + { + if (e->copy < 0) + { + int s = edges.size(); + edges.push_back(Edge()); + edges.push_back(Edge()); + Edge* c = &edges[s]; + Edge* r = &edges[s + 1]; + e->copy = s; + e->reverse->copy = s + 1; + c->reverse = 1; + r->reverse = -1; + c->targetVertex = getVertexCopy(e->target, oldVertices); + r->targetVertex = copied; +#ifdef DEBUG_CONVEX_HULL + printf(" CREATE: Vertex *%d has edge to *%d\n", copied, c->getTargetVertex()); +#endif + } + if (prevCopy >= 0) + { + edges[e->copy].next = prevCopy - e->copy; + } + else + { + firstCopy = e->copy; + } + prevCopy = e->copy; + e = e->next; + } while (e != firstEdge); + edges[firstCopy].next = prevCopy - firstCopy; + } + copied++; + } + + for (int i = 0; i < copied; i++) + { + btConvexHullInternal::Vertex* v = oldVertices[i]; + btConvexHullInternal::Edge* firstEdge = v->edges; + if (firstEdge) + { + btConvexHullInternal::Edge* e = firstEdge; + do + { + if (e->copy >= 0) + { +#ifdef DEBUG_CONVEX_HULL + printf("Vertex *%d has edge to *%d\n", i, edges[e->copy].getTargetVertex()); +#endif + faces.push_back(e->copy); + btConvexHullInternal::Edge* f = e; + do + { +#ifdef DEBUG_CONVEX_HULL + printf(" Face *%d\n", edges[f->copy].getTargetVertex()); +#endif + f->copy = -1; + f = f->reverse->prev; + } while (f != e); + } + e = e->next; + } while (e != firstEdge); + } + } + + return shift; +} + + + + + diff --git a/extern/bullet2/src/LinearMath/btConvexHullComputer.h b/extern/bullet2/src/LinearMath/btConvexHullComputer.h new file mode 100644 index 00000000000..7240ac4fb52 --- /dev/null +++ b/extern/bullet2/src/LinearMath/btConvexHullComputer.h @@ -0,0 +1,103 @@ +/* +Copyright (c) 2011 Ole Kniemeyer, MAXON, www.maxon.net + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef BT_CONVEX_HULL_COMPUTER_H +#define BT_CONVEX_HULL_COMPUTER_H + +#include "btVector3.h" +#include "btAlignedObjectArray.h" + +/// Convex hull implementation based on Preparata and Hong +/// See http://code.google.com/p/bullet/issues/detail?id=275 +/// Ole Kniemeyer, MAXON Computer GmbH +class btConvexHullComputer +{ + private: + btScalar compute(const void* coords, bool doubleCoords, int stride, int count, btScalar shrink, btScalar shrinkClamp); + + public: + + class Edge + { + private: + int next; + int reverse; + int targetVertex; + + friend class btConvexHullComputer; + + public: + int getSourceVertex() const + { + return (this + reverse)->targetVertex; + } + + int getTargetVertex() const + { + return targetVertex; + } + + const Edge* getNextEdgeOfVertex() const // clockwise list of all edges of a vertex + { + return this + next; + } + + const Edge* getNextEdgeOfFace() const // counter-clockwise list of all edges of a face + { + return (this + reverse)->getNextEdgeOfVertex(); + } + + const Edge* getReverseEdge() const + { + return this + reverse; + } + }; + + + // Vertices of the output hull + btAlignedObjectArray<btVector3> vertices; + + // Edges of the output hull + btAlignedObjectArray<Edge> edges; + + // Faces of the convex hull. Each entry is an index into the "edges" array pointing to an edge of the face. Faces are planar n-gons + btAlignedObjectArray<int> faces; + + /* + Compute convex hull of "count" vertices stored in "coords". "stride" is the difference in bytes + between the addresses of consecutive vertices. If "shrink" is positive, the convex hull is shrunken + by that amount (each face is moved by "shrink" length units towards the center along its normal). + If "shrinkClamp" is positive, "shrink" is clamped to not exceed "shrinkClamp * innerRadius", where "innerRadius" + is the minimum distance of a face to the center of the convex hull. + + The returned value is the amount by which the hull has been shrunken. If it is negative, the amount was so large + that the resulting convex hull is empty. + + The output convex hull can be found in the member variables "vertices", "edges", "faces". + */ + btScalar compute(const float* coords, int stride, int count, btScalar shrink, btScalar shrinkClamp) + { + return compute(coords, false, stride, count, shrink, shrinkClamp); + } + + // same as above, but double precision + btScalar compute(const double* coords, int stride, int count, btScalar shrink, btScalar shrinkClamp) + { + return compute(coords, true, stride, count, shrink, shrinkClamp); + } +}; + + +#endif //BT_CONVEX_HULL_COMPUTER_H + diff --git a/extern/bullet2/src/LinearMath/btDefaultMotionState.h b/extern/bullet2/src/LinearMath/btDefaultMotionState.h index 7858a10d219..a6b7ef15ac8 100644 --- a/extern/bullet2/src/LinearMath/btDefaultMotionState.h +++ b/extern/bullet2/src/LinearMath/btDefaultMotionState.h @@ -1,5 +1,5 @@ -#ifndef DEFAULT_MOTION_STATE_H -#define DEFAULT_MOTION_STATE_H +#ifndef BT_DEFAULT_MOTION_STATE_H +#define BT_DEFAULT_MOTION_STATE_H #include "btMotionState.h" @@ -37,4 +37,4 @@ struct btDefaultMotionState : public btMotionState }; -#endif //DEFAULT_MOTION_STATE_H +#endif //BT_DEFAULT_MOTION_STATE_H diff --git a/extern/bullet2/src/LinearMath/btGrahamScan2dConvexHull.h b/extern/bullet2/src/LinearMath/btGrahamScan2dConvexHull.h new file mode 100644 index 00000000000..d7bd3eb8911 --- /dev/null +++ b/extern/bullet2/src/LinearMath/btGrahamScan2dConvexHull.h @@ -0,0 +1,110 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2011 Advanced Micro Devices, Inc. http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + +#ifndef GRAHAM_SCAN_2D_CONVEX_HULL_H +#define GRAHAM_SCAN_2D_CONVEX_HULL_H + + +#include "btVector3.h" +#include "btAlignedObjectArray.h" + +struct GrahamVector2 : public btVector3 +{ + GrahamVector2(const btVector3& org, int orgIndex) + :btVector3(org), + m_orgIndex(orgIndex) + { + } + btScalar m_angle; + int m_orgIndex; +}; + + +struct btAngleCompareFunc { + btVector3 m_anchor; + btAngleCompareFunc(const btVector3& anchor) + : m_anchor(anchor) + { + } + bool operator()(const GrahamVector2& a, const GrahamVector2& b) const { + if (a.m_angle != b.m_angle) + return a.m_angle < b.m_angle; + else + { + btScalar al = (a-m_anchor).length2(); + btScalar bl = (b-m_anchor).length2(); + if (al != bl) + return al < bl; + else + { + return a.m_orgIndex < b.m_orgIndex; + } + } + } +}; + +inline void GrahamScanConvexHull2D(btAlignedObjectArray<GrahamVector2>& originalPoints, btAlignedObjectArray<GrahamVector2>& hull) +{ + if (originalPoints.size()<=1) + { + for (int i=0;i<originalPoints.size();i++) + hull.push_back(originalPoints[0]); + return; + } + //step1 : find anchor point with smallest x/y and move it to first location + //also precompute angles + for (int i=0;i<originalPoints.size();i++) + { + const btVector3& left = originalPoints[i]; + const btVector3& right = originalPoints[0]; + if (left.x() < right.x() || !(right.x() < left.x()) && left.y() < right.y()) + { + originalPoints.swap(0,i); + } + } + + for (int i=0;i<originalPoints.size();i++) + { + btVector3 xvec(1,0,0); + btVector3 ar = originalPoints[i]-originalPoints[0]; + originalPoints[i].m_angle = btCross(xvec, ar).dot(btVector3(0,0,1)) / ar.length(); + } + + //step 2: sort all points, based on 'angle' with this anchor + btAngleCompareFunc comp(originalPoints[0]); + originalPoints.quickSortInternal(comp,1,originalPoints.size()-1); + + int i; + for (i = 0; i<2; i++) + hull.push_back(originalPoints[i]); + + //step 3: keep all 'convex' points and discard concave points (using back tracking) + for (; i != originalPoints.size(); i++) + { + bool isConvex = false; + while (!isConvex&& hull.size()>1) { + btVector3& a = hull[hull.size()-2]; + btVector3& b = hull[hull.size()-1]; + isConvex = btCross(a-b,a-originalPoints[i]).dot(btVector3(0,0,1))> 0; + if (!isConvex) + hull.pop_back(); + else + hull.push_back(originalPoints[i]); + } + } +} + +#endif //GRAHAM_SCAN_2D_CONVEX_HULL_H diff --git a/extern/bullet2/src/LinearMath/btHashMap.h b/extern/bullet2/src/LinearMath/btHashMap.h index e3302b5e9ff..ce07db3ac6b 100644 --- a/extern/bullet2/src/LinearMath/btHashMap.h +++ b/extern/bullet2/src/LinearMath/btHashMap.h @@ -1,3 +1,19 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + #ifndef BT_HASH_MAP_H #define BT_HASH_MAP_H diff --git a/extern/bullet2/src/LinearMath/btIDebugDraw.h b/extern/bullet2/src/LinearMath/btIDebugDraw.h index 48c15806a7d..935502f844f 100644 --- a/extern/bullet2/src/LinearMath/btIDebugDraw.h +++ b/extern/bullet2/src/LinearMath/btIDebugDraw.h @@ -14,8 +14,8 @@ subject to the following restrictions: */ -#ifndef IDEBUG_DRAW__H -#define IDEBUG_DRAW__H +#ifndef BT_IDEBUG_DRAW__H +#define BT_IDEBUG_DRAW__H #include "btVector3.h" #include "btTransform.h" @@ -46,6 +46,7 @@ class btIDebugDraw DBG_DrawConstraints = (1 << 11), DBG_DrawConstraintLimits = (1 << 12), DBG_FastWireframe = (1<<13), + DBG_DrawNormals = (1<<14), DBG_MAX_DEBUG_DRAW_MODE }; @@ -413,5 +414,5 @@ class btIDebugDraw }; -#endif //IDEBUG_DRAW__H +#endif //BT_IDEBUG_DRAW__H diff --git a/extern/bullet2/src/LinearMath/btList.h b/extern/bullet2/src/LinearMath/btList.h index c87b47faf2b..eec80a7064b 100644 --- a/extern/bullet2/src/LinearMath/btList.h +++ b/extern/bullet2/src/LinearMath/btList.h @@ -14,8 +14,8 @@ subject to the following restrictions: -#ifndef GEN_LIST_H -#define GEN_LIST_H +#ifndef BT_GEN_LIST_H +#define BT_GEN_LIST_H class btGEN_Link { public: @@ -67,7 +67,7 @@ private: btGEN_Link m_tail; }; -#endif +#endif //BT_GEN_LIST_H diff --git a/extern/bullet2/src/LinearMath/btMinMax.h b/extern/bullet2/src/LinearMath/btMinMax.h index 80601c1e232..5b436e9ba4e 100644 --- a/extern/bullet2/src/LinearMath/btMinMax.h +++ b/extern/bullet2/src/LinearMath/btMinMax.h @@ -14,10 +14,10 @@ subject to the following restrictions: -#ifndef GEN_MINMAX_H -#define GEN_MINMAX_H +#ifndef BT_GEN_MINMAX_H +#define BT_GEN_MINMAX_H -#include "LinearMath/btScalar.h" +#include "btScalar.h" template <class T> SIMD_FORCE_INLINE const T& btMin(const T& a, const T& b) @@ -68,4 +68,4 @@ SIMD_FORCE_INLINE void btClamp(T& a, const T& lb, const T& ub) } } -#endif +#endif //BT_GEN_MINMAX_H diff --git a/extern/bullet2/src/LinearMath/btPoint3.h b/extern/bullet2/src/LinearMath/btPoint3.h deleted file mode 100644 index a2020e26d12..00000000000 --- a/extern/bullet2/src/LinearMath/btPoint3.h +++ /dev/null @@ -1,24 +0,0 @@ -/* -Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - - -#ifndef btPoint3_H -#define btPoint3_H - -#include "btVector3.h" - -typedef btVector3 btPoint3; - -#endif diff --git a/extern/bullet2/src/LinearMath/btPoolAllocator.h b/extern/bullet2/src/LinearMath/btPoolAllocator.h index e3b4be9b155..ef2084537a2 100644 --- a/extern/bullet2/src/LinearMath/btPoolAllocator.h +++ b/extern/bullet2/src/LinearMath/btPoolAllocator.h @@ -62,6 +62,11 @@ public: return m_maxElements - m_freeCount; } + int getMaxCount() const + { + return m_maxElements; + } + void* allocate(int size) { // release mode fix diff --git a/extern/bullet2/src/LinearMath/btQuadWord.h b/extern/bullet2/src/LinearMath/btQuadWord.h index c657afd2bb1..d5e9daa45a2 100644 --- a/extern/bullet2/src/LinearMath/btQuadWord.h +++ b/extern/bullet2/src/LinearMath/btQuadWord.h @@ -13,8 +13,8 @@ subject to the following restrictions: */ -#ifndef SIMD_QUADWORD_H -#define SIMD_QUADWORD_H +#ifndef BT_SIMD_QUADWORD_H +#define BT_SIMD_QUADWORD_H #include "btScalar.h" #include "btMinMax.h" @@ -177,4 +177,4 @@ protected: }; -#endif //SIMD_QUADWORD_H +#endif //BT_SIMD_QUADWORD_H diff --git a/extern/bullet2/src/LinearMath/btQuaternion.h b/extern/bullet2/src/LinearMath/btQuaternion.h index 15cf5f868d9..ee79f6eaeee 100644 --- a/extern/bullet2/src/LinearMath/btQuaternion.h +++ b/extern/bullet2/src/LinearMath/btQuaternion.h @@ -14,8 +14,8 @@ subject to the following restrictions: -#ifndef SIMD__QUATERNION_H_ -#define SIMD__QUATERNION_H_ +#ifndef BT_SIMD__QUATERNION_H_ +#define BT_SIMD__QUATERNION_H_ #include "btVector3.h" @@ -212,11 +212,12 @@ public: /**@brief Return the axis of the rotation represented by this quaternion */ btVector3 getAxis() const { - btScalar s_squared = btScalar(1.) - btPow(m_floats[3], btScalar(2.)); + btScalar s_squared = 1.f-m_floats[3]*m_floats[3]; + if (s_squared < btScalar(10.) * SIMD_EPSILON) //Check for divide by zero return btVector3(1.0, 0.0, 0.0); // Arbitrary - btScalar s = btSqrt(s_squared); - return btVector3(m_floats[0] / s, m_floats[1] / s, m_floats[2] / s); + btScalar s = 1.f/btSqrt(s_squared); + return btVector3(m_floats[0] * s, m_floats[1] * s, m_floats[2] * s); } /**@brief Return the inverse of this quaternion */ @@ -279,23 +280,25 @@ public: * Slerp interpolates assuming constant velocity. */ btQuaternion slerp(const btQuaternion& q, const btScalar& t) const { - btScalar theta = angle(q); - if (theta != btScalar(0.0)) + btScalar magnitude = btSqrt(length2() * q.length2()); + btAssert(magnitude > btScalar(0)); + + btScalar product = dot(q) / magnitude; + if (btFabs(product) != btScalar(1)) { - btScalar d = btScalar(1.0) / btSin(theta); - btScalar s0 = btSin((btScalar(1.0) - t) * theta); - btScalar s1 = btSin(t * theta); - if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp - return btQuaternion((m_floats[0] * s0 + -q.x() * s1) * d, - (m_floats[1] * s0 + -q.y() * s1) * d, - (m_floats[2] * s0 + -q.z() * s1) * d, - (m_floats[3] * s0 + -q.m_floats[3] * s1) * d); - else - return btQuaternion((m_floats[0] * s0 + q.x() * s1) * d, - (m_floats[1] * s0 + q.y() * s1) * d, - (m_floats[2] * s0 + q.z() * s1) * d, - (m_floats[3] * s0 + q.m_floats[3] * s1) * d); - + // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp + const btScalar sign = (product < 0) ? btScalar(-1) : btScalar(1); + + const btScalar theta = btAcos(sign * product); + const btScalar s1 = btSin(sign * t * theta); + const btScalar d = btScalar(1.0) / btSin(theta); + const btScalar s0 = btSin((btScalar(1.0) - t) * theta); + + return btQuaternion( + (m_floats[0] * s0 + q.x() * s1) * d, + (m_floats[1] * s0 + q.y() * s1) * d, + (m_floats[2] * s0 + q.z() * s1) * d, + (m_floats[3] * s0 + q.m_floats[3] * s1) * d); } else { @@ -315,12 +318,6 @@ public: }; -/**@brief Return the negative of a quaternion */ -SIMD_FORCE_INLINE btQuaternion -operator-(const btQuaternion& q) -{ - return btQuaternion(-q.x(), -q.y(), -q.z(), -q.w()); -} @@ -426,7 +423,7 @@ shortestArcQuatNormalize2(btVector3& v0,btVector3& v1) return shortestArcQuat(v0,v1); } -#endif +#endif //BT_SIMD__QUATERNION_H_ diff --git a/extern/bullet2/src/LinearMath/btQuickprof.cpp b/extern/bullet2/src/LinearMath/btQuickprof.cpp index 14534068a0d..544aee89d02 100644 --- a/extern/bullet2/src/LinearMath/btQuickprof.cpp +++ b/extern/bullet2/src/LinearMath/btQuickprof.cpp @@ -276,7 +276,8 @@ CProfileNode::CProfileNode( const char * name, CProfileNode * parent ) : RecursionCounter( 0 ), Parent( parent ), Child( NULL ), - Sibling( NULL ) + Sibling( NULL ), + m_userPtr(0) { Reset(); } diff --git a/extern/bullet2/src/LinearMath/btQuickprof.h b/extern/bullet2/src/LinearMath/btQuickprof.h index adfbbe6149c..93f3f4a60fa 100644 --- a/extern/bullet2/src/LinearMath/btQuickprof.h +++ b/extern/bullet2/src/LinearMath/btQuickprof.h @@ -12,8 +12,8 @@ -#ifndef QUICK_PROF_H -#define QUICK_PROF_H +#ifndef BT_QUICK_PROF_H +#define BT_QUICK_PROF_H //To disable built-in profiling, please comment out next line //#define BT_NO_PROFILE 1 @@ -82,7 +82,8 @@ public: const char * Get_Name( void ) { return Name; } int Get_Total_Calls( void ) { return TotalCalls; } float Get_Total_Time( void ) { return TotalTime; } - + void* GetUserPointer() const {return m_userPtr;} + void SetUserPointer(void* ptr) { m_userPtr = ptr;} protected: const char * Name; @@ -94,6 +95,7 @@ protected: CProfileNode * Parent; CProfileNode * Child; CProfileNode * Sibling; + void* m_userPtr; }; ///An iterator to navigate through the tree @@ -115,15 +117,20 @@ public: int Get_Current_Total_Calls( void ) { return CurrentChild->Get_Total_Calls(); } float Get_Current_Total_Time( void ) { return CurrentChild->Get_Total_Time(); } + void* Get_Current_UserPointer( void ) { return CurrentChild->GetUserPointer(); } + void Set_Current_UserPointer(void* ptr) {CurrentChild->SetUserPointer(ptr);} // Access the current parent const char * Get_Current_Parent_Name( void ) { return CurrentParent->Get_Name(); } int Get_Current_Parent_Total_Calls( void ) { return CurrentParent->Get_Total_Calls(); } float Get_Current_Parent_Total_Time( void ) { return CurrentParent->Get_Total_Time(); } + + protected: CProfileNode * CurrentParent; CProfileNode * CurrentChild; + CProfileIterator( CProfileNode * start ); friend class CProfileManager; @@ -191,6 +198,6 @@ public: -#endif //QUICK_PROF_H +#endif //BT_QUICK_PROF_H diff --git a/extern/bullet2/src/LinearMath/btRandom.h b/extern/bullet2/src/LinearMath/btRandom.h index fdf65e01caf..4cbfc6bfe9f 100644 --- a/extern/bullet2/src/LinearMath/btRandom.h +++ b/extern/bullet2/src/LinearMath/btRandom.h @@ -14,8 +14,8 @@ subject to the following restrictions: -#ifndef GEN_RANDOM_H -#define GEN_RANDOM_H +#ifndef BT_GEN_RANDOM_H +#define BT_GEN_RANDOM_H #ifdef MT19937 @@ -38,5 +38,5 @@ SIMD_FORCE_INLINE unsigned int GEN_rand() { return rand(); } #endif -#endif +#endif //BT_GEN_RANDOM_H diff --git a/extern/bullet2/src/LinearMath/btScalar.h b/extern/bullet2/src/LinearMath/btScalar.h index 6ba6c5ad30d..ecae972243c 100644 --- a/extern/bullet2/src/LinearMath/btScalar.h +++ b/extern/bullet2/src/LinearMath/btScalar.h @@ -14,8 +14,8 @@ subject to the following restrictions: -#ifndef SIMD___SCALAR_H -#define SIMD___SCALAR_H +#ifndef BT_SCALAR_H +#define BT_SCALAR_H #ifdef BT_MANAGED_CODE //Aligned data types not supported in managed code @@ -25,12 +25,10 @@ subject to the following restrictions: #include <math.h> #include <stdlib.h>//size_t for MSVC 6.0 -#include <cstdlib> -#include <cfloat> #include <float.h> -/* SVN $Revision: 35500 $ on $Date: 2011-03-12 21:34:17 +0100 (Sat, 12 Mar 2011) $ from http://bullet.googlecode.com*/ -#define BT_BULLET_VERSION 278 +/* SVN $Revision$ on $Date$ from http://bullet.googlecode.com*/ +#define BT_BULLET_VERSION 280 inline int btGetVersion() { @@ -521,4 +519,29 @@ struct btTypedObject return m_objectType; } }; -#endif //SIMD___SCALAR_H + + + +///align a pointer to the provided alignment, upwards +template <typename T>T* btAlignPointer(T* unalignedPtr, size_t alignment) +{ + + struct btConvertPointerSizeT + { + union + { + T* ptr; + size_t integer; + }; + }; + btConvertPointerSizeT converter; + + + const size_t bit_mask = ~(alignment - 1); + converter.ptr = unalignedPtr; + converter.integer += alignment-1; + converter.integer &= bit_mask; + return converter.ptr; +} + +#endif //BT_SCALAR_H diff --git a/extern/bullet2/src/LinearMath/btSerializer.cpp b/extern/bullet2/src/LinearMath/btSerializer.cpp index c6d387e6133..49c25b7ea2a 100644 --- a/extern/bullet2/src/LinearMath/btSerializer.cpp +++ b/extern/bullet2/src/LinearMath/btSerializer.cpp @@ -1,5 +1,5 @@ -unsigned char sBulletDNAstr[]= { -83,68,78,65,78,65,77,69,42,1,0,0,109,95,115,105,122,101,0,109, +char sBulletDNAstr[]= { +83,68,78,65,78,65,77,69,44,1,0,0,109,95,115,105,122,101,0,109, 95,99,97,112,97,99,105,116,121,0,42,109,95,100,97,116,97,0,109,95, 99,111,108,108,105,115,105,111,110,83,104,97,112,101,115,0,109,95,99,111, 108,108,105,115,105,111,110,79,98,106,101,99,116,115,0,109,95,99,111,110, @@ -130,292 +130,297 @@ unsigned char sBulletDNAstr[]= { 112,108,105,101,100,73,109,112,117,108,115,101,0,109,95,100,98,103,68,114, 97,119,83,105,122,101,0,109,95,100,105,115,97,98,108,101,67,111,108,108, 105,115,105,111,110,115,66,101,116,119,101,101,110,76,105,110,107,101,100,66, -111,100,105,101,115,0,109,95,112,97,100,52,91,52,93,0,109,95,116,121, -112,101,67,111,110,115,116,114,97,105,110,116,68,97,116,97,0,109,95,112, -105,118,111,116,73,110,65,0,109,95,112,105,118,111,116,73,110,66,0,109, -95,114,98,65,70,114,97,109,101,0,109,95,114,98,66,70,114,97,109,101, -0,109,95,117,115,101,82,101,102,101,114,101,110,99,101,70,114,97,109,101, -65,0,109,95,97,110,103,117,108,97,114,79,110,108,121,0,109,95,101,110, -97,98,108,101,65,110,103,117,108,97,114,77,111,116,111,114,0,109,95,109, -111,116,111,114,84,97,114,103,101,116,86,101,108,111,99,105,116,121,0,109, -95,109,97,120,77,111,116,111,114,73,109,112,117,108,115,101,0,109,95,108, -111,119,101,114,76,105,109,105,116,0,109,95,117,112,112,101,114,76,105,109, -105,116,0,109,95,108,105,109,105,116,83,111,102,116,110,101,115,115,0,109, -95,98,105,97,115,70,97,99,116,111,114,0,109,95,114,101,108,97,120,97, -116,105,111,110,70,97,99,116,111,114,0,109,95,115,119,105,110,103,83,112, -97,110,49,0,109,95,115,119,105,110,103,83,112,97,110,50,0,109,95,116, -119,105,115,116,83,112,97,110,0,109,95,100,97,109,112,105,110,103,0,109, -95,108,105,110,101,97,114,85,112,112,101,114,76,105,109,105,116,0,109,95, -108,105,110,101,97,114,76,111,119,101,114,76,105,109,105,116,0,109,95,97, -110,103,117,108,97,114,85,112,112,101,114,76,105,109,105,116,0,109,95,97, -110,103,117,108,97,114,76,111,119,101,114,76,105,109,105,116,0,109,95,117, -115,101,76,105,110,101,97,114,82,101,102,101,114,101,110,99,101,70,114,97, -109,101,65,0,109,95,117,115,101,79,102,102,115,101,116,70,111,114,67,111, -110,115,116,114,97,105,110,116,70,114,97,109,101,0,109,95,54,100,111,102, -68,97,116,97,0,109,95,115,112,114,105,110,103,69,110,97,98,108,101,100, -91,54,93,0,109,95,101,113,117,105,108,105,98,114,105,117,109,80,111,105, -110,116,91,54,93,0,109,95,115,112,114,105,110,103,83,116,105,102,102,110, -101,115,115,91,54,93,0,109,95,115,112,114,105,110,103,68,97,109,112,105, -110,103,91,54,93,0,109,95,108,105,110,101,97,114,83,116,105,102,102,110, -101,115,115,0,109,95,97,110,103,117,108,97,114,83,116,105,102,102,110,101, -115,115,0,109,95,118,111,108,117,109,101,83,116,105,102,102,110,101,115,115, -0,42,109,95,109,97,116,101,114,105,97,108,0,109,95,112,111,115,105,116, -105,111,110,0,109,95,112,114,101,118,105,111,117,115,80,111,115,105,116,105, -111,110,0,109,95,118,101,108,111,99,105,116,121,0,109,95,97,99,99,117, -109,117,108,97,116,101,100,70,111,114,99,101,0,109,95,110,111,114,109,97, -108,0,109,95,97,114,101,97,0,109,95,97,116,116,97,99,104,0,109,95, -110,111,100,101,73,110,100,105,99,101,115,91,50,93,0,109,95,114,101,115, -116,76,101,110,103,116,104,0,109,95,98,98,101,110,100,105,110,103,0,109, -95,110,111,100,101,73,110,100,105,99,101,115,91,51,93,0,109,95,114,101, -115,116,65,114,101,97,0,109,95,99,48,91,52,93,0,109,95,110,111,100, -101,73,110,100,105,99,101,115,91,52,93,0,109,95,114,101,115,116,86,111, -108,117,109,101,0,109,95,99,49,0,109,95,99,50,0,109,95,99,48,0, -109,95,108,111,99,97,108,70,114,97,109,101,0,42,109,95,114,105,103,105, -100,66,111,100,121,0,109,95,110,111,100,101,73,110,100,101,120,0,109,95, -97,101,114,111,77,111,100,101,108,0,109,95,98,97,117,109,103,97,114,116, -101,0,109,95,100,114,97,103,0,109,95,108,105,102,116,0,109,95,112,114, -101,115,115,117,114,101,0,109,95,118,111,108,117,109,101,0,109,95,100,121, -110,97,109,105,99,70,114,105,99,116,105,111,110,0,109,95,112,111,115,101, -77,97,116,99,104,0,109,95,114,105,103,105,100,67,111,110,116,97,99,116, -72,97,114,100,110,101,115,115,0,109,95,107,105,110,101,116,105,99,67,111, -110,116,97,99,116,72,97,114,100,110,101,115,115,0,109,95,115,111,102,116, -67,111,110,116,97,99,116,72,97,114,100,110,101,115,115,0,109,95,97,110, -99,104,111,114,72,97,114,100,110,101,115,115,0,109,95,115,111,102,116,82, -105,103,105,100,67,108,117,115,116,101,114,72,97,114,100,110,101,115,115,0, -109,95,115,111,102,116,75,105,110,101,116,105,99,67,108,117,115,116,101,114, -72,97,114,100,110,101,115,115,0,109,95,115,111,102,116,83,111,102,116,67, -108,117,115,116,101,114,72,97,114,100,110,101,115,115,0,109,95,115,111,102, -116,82,105,103,105,100,67,108,117,115,116,101,114,73,109,112,117,108,115,101, -83,112,108,105,116,0,109,95,115,111,102,116,75,105,110,101,116,105,99,67, -108,117,115,116,101,114,73,109,112,117,108,115,101,83,112,108,105,116,0,109, -95,115,111,102,116,83,111,102,116,67,108,117,115,116,101,114,73,109,112,117, -108,115,101,83,112,108,105,116,0,109,95,109,97,120,86,111,108,117,109,101, -0,109,95,116,105,109,101,83,99,97,108,101,0,109,95,118,101,108,111,99, -105,116,121,73,116,101,114,97,116,105,111,110,115,0,109,95,112,111,115,105, -116,105,111,110,73,116,101,114,97,116,105,111,110,115,0,109,95,100,114,105, -102,116,73,116,101,114,97,116,105,111,110,115,0,109,95,99,108,117,115,116, -101,114,73,116,101,114,97,116,105,111,110,115,0,109,95,114,111,116,0,109, -95,115,99,97,108,101,0,109,95,97,113,113,0,109,95,99,111,109,0,42, -109,95,112,111,115,105,116,105,111,110,115,0,42,109,95,119,101,105,103,104, -116,115,0,109,95,110,117,109,80,111,115,105,116,105,111,110,115,0,109,95, -110,117,109,87,101,105,103,116,115,0,109,95,98,118,111,108,117,109,101,0, -109,95,98,102,114,97,109,101,0,109,95,102,114,97,109,101,120,102,111,114, -109,0,109,95,108,111,99,105,105,0,109,95,105,110,118,119,105,0,109,95, -118,105,109,112,117,108,115,101,115,91,50,93,0,109,95,100,105,109,112,117, -108,115,101,115,91,50,93,0,109,95,108,118,0,109,95,97,118,0,42,109, -95,102,114,97,109,101,114,101,102,115,0,42,109,95,110,111,100,101,73,110, -100,105,99,101,115,0,42,109,95,109,97,115,115,101,115,0,109,95,110,117, -109,70,114,97,109,101,82,101,102,115,0,109,95,110,117,109,78,111,100,101, -115,0,109,95,110,117,109,77,97,115,115,101,115,0,109,95,105,100,109,97, -115,115,0,109,95,105,109,97,115,115,0,109,95,110,118,105,109,112,117,108, -115,101,115,0,109,95,110,100,105,109,112,117,108,115,101,115,0,109,95,110, -100,97,109,112,105,110,103,0,109,95,108,100,97,109,112,105,110,103,0,109, -95,97,100,97,109,112,105,110,103,0,109,95,109,97,116,99,104,105,110,103, -0,109,95,109,97,120,83,101,108,102,67,111,108,108,105,115,105,111,110,73, -109,112,117,108,115,101,0,109,95,115,101,108,102,67,111,108,108,105,115,105, -111,110,73,109,112,117,108,115,101,70,97,99,116,111,114,0,109,95,99,111, -110,116,97,105,110,115,65,110,99,104,111,114,0,109,95,99,111,108,108,105, -100,101,0,109,95,99,108,117,115,116,101,114,73,110,100,101,120,0,42,109, -95,98,111,100,121,65,0,42,109,95,98,111,100,121,66,0,109,95,114,101, -102,115,91,50,93,0,109,95,99,102,109,0,109,95,101,114,112,0,109,95, -115,112,108,105,116,0,109,95,100,101,108,101,116,101,0,109,95,114,101,108, -80,111,115,105,116,105,111,110,91,50,93,0,109,95,98,111,100,121,65,116, -121,112,101,0,109,95,98,111,100,121,66,116,121,112,101,0,109,95,106,111, -105,110,116,84,121,112,101,0,42,109,95,112,111,115,101,0,42,42,109,95, -109,97,116,101,114,105,97,108,115,0,42,109,95,110,111,100,101,115,0,42, -109,95,108,105,110,107,115,0,42,109,95,102,97,99,101,115,0,42,109,95, -116,101,116,114,97,104,101,100,114,97,0,42,109,95,97,110,99,104,111,114, -115,0,42,109,95,99,108,117,115,116,101,114,115,0,42,109,95,106,111,105, -110,116,115,0,109,95,110,117,109,77,97,116,101,114,105,97,108,115,0,109, -95,110,117,109,76,105,110,107,115,0,109,95,110,117,109,70,97,99,101,115, -0,109,95,110,117,109,84,101,116,114,97,104,101,100,114,97,0,109,95,110, -117,109,65,110,99,104,111,114,115,0,109,95,110,117,109,67,108,117,115,116, -101,114,115,0,109,95,110,117,109,74,111,105,110,116,115,0,109,95,99,111, -110,102,105,103,0,0,0,0,84,89,80,69,72,0,0,0,99,104,97,114, -0,117,99,104,97,114,0,115,104,111,114,116,0,117,115,104,111,114,116,0, -105,110,116,0,108,111,110,103,0,117,108,111,110,103,0,102,108,111,97,116, -0,100,111,117,98,108,101,0,118,111,105,100,0,80,111,105,110,116,101,114, -65,114,114,97,121,0,98,116,80,104,121,115,105,99,115,83,121,115,116,101, -109,0,76,105,115,116,66,97,115,101,0,98,116,86,101,99,116,111,114,51, -70,108,111,97,116,68,97,116,97,0,98,116,86,101,99,116,111,114,51,68, -111,117,98,108,101,68,97,116,97,0,98,116,77,97,116,114,105,120,51,120, -51,70,108,111,97,116,68,97,116,97,0,98,116,77,97,116,114,105,120,51, -120,51,68,111,117,98,108,101,68,97,116,97,0,98,116,84,114,97,110,115, -102,111,114,109,70,108,111,97,116,68,97,116,97,0,98,116,84,114,97,110, -115,102,111,114,109,68,111,117,98,108,101,68,97,116,97,0,98,116,66,118, -104,83,117,98,116,114,101,101,73,110,102,111,68,97,116,97,0,98,116,79, -112,116,105,109,105,122,101,100,66,118,104,78,111,100,101,70,108,111,97,116, -68,97,116,97,0,98,116,79,112,116,105,109,105,122,101,100,66,118,104,78, -111,100,101,68,111,117,98,108,101,68,97,116,97,0,98,116,81,117,97,110, -116,105,122,101,100,66,118,104,78,111,100,101,68,97,116,97,0,98,116,81, -117,97,110,116,105,122,101,100,66,118,104,70,108,111,97,116,68,97,116,97, -0,98,116,81,117,97,110,116,105,122,101,100,66,118,104,68,111,117,98,108, -101,68,97,116,97,0,98,116,67,111,108,108,105,115,105,111,110,83,104,97, -112,101,68,97,116,97,0,98,116,83,116,97,116,105,99,80,108,97,110,101, -83,104,97,112,101,68,97,116,97,0,98,116,67,111,110,118,101,120,73,110, -116,101,114,110,97,108,83,104,97,112,101,68,97,116,97,0,98,116,80,111, -115,105,116,105,111,110,65,110,100,82,97,100,105,117,115,0,98,116,77,117, -108,116,105,83,112,104,101,114,101,83,104,97,112,101,68,97,116,97,0,98, -116,73,110,116,73,110,100,101,120,68,97,116,97,0,98,116,83,104,111,114, -116,73,110,116,73,110,100,101,120,68,97,116,97,0,98,116,83,104,111,114, -116,73,110,116,73,110,100,101,120,84,114,105,112,108,101,116,68,97,116,97, -0,98,116,67,104,97,114,73,110,100,101,120,84,114,105,112,108,101,116,68, -97,116,97,0,98,116,77,101,115,104,80,97,114,116,68,97,116,97,0,98, -116,83,116,114,105,100,105,110,103,77,101,115,104,73,110,116,101,114,102,97, -99,101,68,97,116,97,0,98,116,84,114,105,97,110,103,108,101,77,101,115, -104,83,104,97,112,101,68,97,116,97,0,98,116,84,114,105,97,110,103,108, -101,73,110,102,111,77,97,112,68,97,116,97,0,98,116,83,99,97,108,101, -100,84,114,105,97,110,103,108,101,77,101,115,104,83,104,97,112,101,68,97, -116,97,0,98,116,67,111,109,112,111,117,110,100,83,104,97,112,101,67,104, -105,108,100,68,97,116,97,0,98,116,67,111,109,112,111,117,110,100,83,104, -97,112,101,68,97,116,97,0,98,116,67,121,108,105,110,100,101,114,83,104, -97,112,101,68,97,116,97,0,98,116,67,97,112,115,117,108,101,83,104,97, +111,100,105,101,115,0,109,95,111,118,101,114,114,105,100,101,78,117,109,83, +111,108,118,101,114,73,116,101,114,97,116,105,111,110,115,0,109,95,98,114, +101,97,107,105,110,103,73,109,112,117,108,115,101,84,104,114,101,115,104,111, +108,100,0,109,95,105,115,69,110,97,98,108,101,100,0,109,95,116,121,112, +101,67,111,110,115,116,114,97,105,110,116,68,97,116,97,0,109,95,112,105, +118,111,116,73,110,65,0,109,95,112,105,118,111,116,73,110,66,0,109,95, +114,98,65,70,114,97,109,101,0,109,95,114,98,66,70,114,97,109,101,0, +109,95,117,115,101,82,101,102,101,114,101,110,99,101,70,114,97,109,101,65, +0,109,95,97,110,103,117,108,97,114,79,110,108,121,0,109,95,101,110,97, +98,108,101,65,110,103,117,108,97,114,77,111,116,111,114,0,109,95,109,111, +116,111,114,84,97,114,103,101,116,86,101,108,111,99,105,116,121,0,109,95, +109,97,120,77,111,116,111,114,73,109,112,117,108,115,101,0,109,95,108,111, +119,101,114,76,105,109,105,116,0,109,95,117,112,112,101,114,76,105,109,105, +116,0,109,95,108,105,109,105,116,83,111,102,116,110,101,115,115,0,109,95, +98,105,97,115,70,97,99,116,111,114,0,109,95,114,101,108,97,120,97,116, +105,111,110,70,97,99,116,111,114,0,109,95,115,119,105,110,103,83,112,97, +110,49,0,109,95,115,119,105,110,103,83,112,97,110,50,0,109,95,116,119, +105,115,116,83,112,97,110,0,109,95,100,97,109,112,105,110,103,0,109,95, +108,105,110,101,97,114,85,112,112,101,114,76,105,109,105,116,0,109,95,108, +105,110,101,97,114,76,111,119,101,114,76,105,109,105,116,0,109,95,97,110, +103,117,108,97,114,85,112,112,101,114,76,105,109,105,116,0,109,95,97,110, +103,117,108,97,114,76,111,119,101,114,76,105,109,105,116,0,109,95,117,115, +101,76,105,110,101,97,114,82,101,102,101,114,101,110,99,101,70,114,97,109, +101,65,0,109,95,117,115,101,79,102,102,115,101,116,70,111,114,67,111,110, +115,116,114,97,105,110,116,70,114,97,109,101,0,109,95,54,100,111,102,68, +97,116,97,0,109,95,115,112,114,105,110,103,69,110,97,98,108,101,100,91, +54,93,0,109,95,101,113,117,105,108,105,98,114,105,117,109,80,111,105,110, +116,91,54,93,0,109,95,115,112,114,105,110,103,83,116,105,102,102,110,101, +115,115,91,54,93,0,109,95,115,112,114,105,110,103,68,97,109,112,105,110, +103,91,54,93,0,109,95,108,105,110,101,97,114,83,116,105,102,102,110,101, +115,115,0,109,95,97,110,103,117,108,97,114,83,116,105,102,102,110,101,115, +115,0,109,95,118,111,108,117,109,101,83,116,105,102,102,110,101,115,115,0, +42,109,95,109,97,116,101,114,105,97,108,0,109,95,112,111,115,105,116,105, +111,110,0,109,95,112,114,101,118,105,111,117,115,80,111,115,105,116,105,111, +110,0,109,95,118,101,108,111,99,105,116,121,0,109,95,97,99,99,117,109, +117,108,97,116,101,100,70,111,114,99,101,0,109,95,110,111,114,109,97,108, +0,109,95,97,114,101,97,0,109,95,97,116,116,97,99,104,0,109,95,110, +111,100,101,73,110,100,105,99,101,115,91,50,93,0,109,95,114,101,115,116, +76,101,110,103,116,104,0,109,95,98,98,101,110,100,105,110,103,0,109,95, +110,111,100,101,73,110,100,105,99,101,115,91,51,93,0,109,95,114,101,115, +116,65,114,101,97,0,109,95,99,48,91,52,93,0,109,95,110,111,100,101, +73,110,100,105,99,101,115,91,52,93,0,109,95,114,101,115,116,86,111,108, +117,109,101,0,109,95,99,49,0,109,95,99,50,0,109,95,99,48,0,109, +95,108,111,99,97,108,70,114,97,109,101,0,42,109,95,114,105,103,105,100, +66,111,100,121,0,109,95,110,111,100,101,73,110,100,101,120,0,109,95,97, +101,114,111,77,111,100,101,108,0,109,95,98,97,117,109,103,97,114,116,101, +0,109,95,100,114,97,103,0,109,95,108,105,102,116,0,109,95,112,114,101, +115,115,117,114,101,0,109,95,118,111,108,117,109,101,0,109,95,100,121,110, +97,109,105,99,70,114,105,99,116,105,111,110,0,109,95,112,111,115,101,77, +97,116,99,104,0,109,95,114,105,103,105,100,67,111,110,116,97,99,116,72, +97,114,100,110,101,115,115,0,109,95,107,105,110,101,116,105,99,67,111,110, +116,97,99,116,72,97,114,100,110,101,115,115,0,109,95,115,111,102,116,67, +111,110,116,97,99,116,72,97,114,100,110,101,115,115,0,109,95,97,110,99, +104,111,114,72,97,114,100,110,101,115,115,0,109,95,115,111,102,116,82,105, +103,105,100,67,108,117,115,116,101,114,72,97,114,100,110,101,115,115,0,109, +95,115,111,102,116,75,105,110,101,116,105,99,67,108,117,115,116,101,114,72, +97,114,100,110,101,115,115,0,109,95,115,111,102,116,83,111,102,116,67,108, +117,115,116,101,114,72,97,114,100,110,101,115,115,0,109,95,115,111,102,116, +82,105,103,105,100,67,108,117,115,116,101,114,73,109,112,117,108,115,101,83, +112,108,105,116,0,109,95,115,111,102,116,75,105,110,101,116,105,99,67,108, +117,115,116,101,114,73,109,112,117,108,115,101,83,112,108,105,116,0,109,95, +115,111,102,116,83,111,102,116,67,108,117,115,116,101,114,73,109,112,117,108, +115,101,83,112,108,105,116,0,109,95,109,97,120,86,111,108,117,109,101,0, +109,95,116,105,109,101,83,99,97,108,101,0,109,95,118,101,108,111,99,105, +116,121,73,116,101,114,97,116,105,111,110,115,0,109,95,112,111,115,105,116, +105,111,110,73,116,101,114,97,116,105,111,110,115,0,109,95,100,114,105,102, +116,73,116,101,114,97,116,105,111,110,115,0,109,95,99,108,117,115,116,101, +114,73,116,101,114,97,116,105,111,110,115,0,109,95,114,111,116,0,109,95, +115,99,97,108,101,0,109,95,97,113,113,0,109,95,99,111,109,0,42,109, +95,112,111,115,105,116,105,111,110,115,0,42,109,95,119,101,105,103,104,116, +115,0,109,95,110,117,109,80,111,115,105,116,105,111,110,115,0,109,95,110, +117,109,87,101,105,103,116,115,0,109,95,98,118,111,108,117,109,101,0,109, +95,98,102,114,97,109,101,0,109,95,102,114,97,109,101,120,102,111,114,109, +0,109,95,108,111,99,105,105,0,109,95,105,110,118,119,105,0,109,95,118, +105,109,112,117,108,115,101,115,91,50,93,0,109,95,100,105,109,112,117,108, +115,101,115,91,50,93,0,109,95,108,118,0,109,95,97,118,0,42,109,95, +102,114,97,109,101,114,101,102,115,0,42,109,95,110,111,100,101,73,110,100, +105,99,101,115,0,42,109,95,109,97,115,115,101,115,0,109,95,110,117,109, +70,114,97,109,101,82,101,102,115,0,109,95,110,117,109,78,111,100,101,115, +0,109,95,110,117,109,77,97,115,115,101,115,0,109,95,105,100,109,97,115, +115,0,109,95,105,109,97,115,115,0,109,95,110,118,105,109,112,117,108,115, +101,115,0,109,95,110,100,105,109,112,117,108,115,101,115,0,109,95,110,100, +97,109,112,105,110,103,0,109,95,108,100,97,109,112,105,110,103,0,109,95, +97,100,97,109,112,105,110,103,0,109,95,109,97,116,99,104,105,110,103,0, +109,95,109,97,120,83,101,108,102,67,111,108,108,105,115,105,111,110,73,109, +112,117,108,115,101,0,109,95,115,101,108,102,67,111,108,108,105,115,105,111, +110,73,109,112,117,108,115,101,70,97,99,116,111,114,0,109,95,99,111,110, +116,97,105,110,115,65,110,99,104,111,114,0,109,95,99,111,108,108,105,100, +101,0,109,95,99,108,117,115,116,101,114,73,110,100,101,120,0,42,109,95, +98,111,100,121,65,0,42,109,95,98,111,100,121,66,0,109,95,114,101,102, +115,91,50,93,0,109,95,99,102,109,0,109,95,101,114,112,0,109,95,115, +112,108,105,116,0,109,95,100,101,108,101,116,101,0,109,95,114,101,108,80, +111,115,105,116,105,111,110,91,50,93,0,109,95,98,111,100,121,65,116,121, +112,101,0,109,95,98,111,100,121,66,116,121,112,101,0,109,95,106,111,105, +110,116,84,121,112,101,0,42,109,95,112,111,115,101,0,42,42,109,95,109, +97,116,101,114,105,97,108,115,0,42,109,95,110,111,100,101,115,0,42,109, +95,108,105,110,107,115,0,42,109,95,102,97,99,101,115,0,42,109,95,116, +101,116,114,97,104,101,100,114,97,0,42,109,95,97,110,99,104,111,114,115, +0,42,109,95,99,108,117,115,116,101,114,115,0,42,109,95,106,111,105,110, +116,115,0,109,95,110,117,109,77,97,116,101,114,105,97,108,115,0,109,95, +110,117,109,76,105,110,107,115,0,109,95,110,117,109,70,97,99,101,115,0, +109,95,110,117,109,84,101,116,114,97,104,101,100,114,97,0,109,95,110,117, +109,65,110,99,104,111,114,115,0,109,95,110,117,109,67,108,117,115,116,101, +114,115,0,109,95,110,117,109,74,111,105,110,116,115,0,109,95,99,111,110, +102,105,103,0,84,89,80,69,72,0,0,0,99,104,97,114,0,117,99,104, +97,114,0,115,104,111,114,116,0,117,115,104,111,114,116,0,105,110,116,0, +108,111,110,103,0,117,108,111,110,103,0,102,108,111,97,116,0,100,111,117, +98,108,101,0,118,111,105,100,0,80,111,105,110,116,101,114,65,114,114,97, +121,0,98,116,80,104,121,115,105,99,115,83,121,115,116,101,109,0,76,105, +115,116,66,97,115,101,0,98,116,86,101,99,116,111,114,51,70,108,111,97, +116,68,97,116,97,0,98,116,86,101,99,116,111,114,51,68,111,117,98,108, +101,68,97,116,97,0,98,116,77,97,116,114,105,120,51,120,51,70,108,111, +97,116,68,97,116,97,0,98,116,77,97,116,114,105,120,51,120,51,68,111, +117,98,108,101,68,97,116,97,0,98,116,84,114,97,110,115,102,111,114,109, +70,108,111,97,116,68,97,116,97,0,98,116,84,114,97,110,115,102,111,114, +109,68,111,117,98,108,101,68,97,116,97,0,98,116,66,118,104,83,117,98, +116,114,101,101,73,110,102,111,68,97,116,97,0,98,116,79,112,116,105,109, +105,122,101,100,66,118,104,78,111,100,101,70,108,111,97,116,68,97,116,97, +0,98,116,79,112,116,105,109,105,122,101,100,66,118,104,78,111,100,101,68, +111,117,98,108,101,68,97,116,97,0,98,116,81,117,97,110,116,105,122,101, +100,66,118,104,78,111,100,101,68,97,116,97,0,98,116,81,117,97,110,116, +105,122,101,100,66,118,104,70,108,111,97,116,68,97,116,97,0,98,116,81, +117,97,110,116,105,122,101,100,66,118,104,68,111,117,98,108,101,68,97,116, +97,0,98,116,67,111,108,108,105,115,105,111,110,83,104,97,112,101,68,97, +116,97,0,98,116,83,116,97,116,105,99,80,108,97,110,101,83,104,97,112, +101,68,97,116,97,0,98,116,67,111,110,118,101,120,73,110,116,101,114,110, +97,108,83,104,97,112,101,68,97,116,97,0,98,116,80,111,115,105,116,105, +111,110,65,110,100,82,97,100,105,117,115,0,98,116,77,117,108,116,105,83, +112,104,101,114,101,83,104,97,112,101,68,97,116,97,0,98,116,73,110,116, +73,110,100,101,120,68,97,116,97,0,98,116,83,104,111,114,116,73,110,116, +73,110,100,101,120,68,97,116,97,0,98,116,83,104,111,114,116,73,110,116, +73,110,100,101,120,84,114,105,112,108,101,116,68,97,116,97,0,98,116,67, +104,97,114,73,110,100,101,120,84,114,105,112,108,101,116,68,97,116,97,0, +98,116,77,101,115,104,80,97,114,116,68,97,116,97,0,98,116,83,116,114, +105,100,105,110,103,77,101,115,104,73,110,116,101,114,102,97,99,101,68,97, +116,97,0,98,116,84,114,105,97,110,103,108,101,77,101,115,104,83,104,97, 112,101,68,97,116,97,0,98,116,84,114,105,97,110,103,108,101,73,110,102, -111,68,97,116,97,0,98,116,71,73,109,112,97,99,116,77,101,115,104,83, -104,97,112,101,68,97,116,97,0,98,116,67,111,110,118,101,120,72,117,108, -108,83,104,97,112,101,68,97,116,97,0,98,116,67,111,108,108,105,115,105, -111,110,79,98,106,101,99,116,68,111,117,98,108,101,68,97,116,97,0,98, -116,67,111,108,108,105,115,105,111,110,79,98,106,101,99,116,70,108,111,97, -116,68,97,116,97,0,98,116,82,105,103,105,100,66,111,100,121,70,108,111, -97,116,68,97,116,97,0,98,116,82,105,103,105,100,66,111,100,121,68,111, -117,98,108,101,68,97,116,97,0,98,116,67,111,110,115,116,114,97,105,110, -116,73,110,102,111,49,0,98,116,84,121,112,101,100,67,111,110,115,116,114, -97,105,110,116,68,97,116,97,0,98,116,82,105,103,105,100,66,111,100,121, -68,97,116,97,0,98,116,80,111,105,110,116,50,80,111,105,110,116,67,111, -110,115,116,114,97,105,110,116,70,108,111,97,116,68,97,116,97,0,98,116, -80,111,105,110,116,50,80,111,105,110,116,67,111,110,115,116,114,97,105,110, -116,68,111,117,98,108,101,68,97,116,97,0,98,116,72,105,110,103,101,67, -111,110,115,116,114,97,105,110,116,68,111,117,98,108,101,68,97,116,97,0, -98,116,72,105,110,103,101,67,111,110,115,116,114,97,105,110,116,70,108,111, -97,116,68,97,116,97,0,98,116,67,111,110,101,84,119,105,115,116,67,111, -110,115,116,114,97,105,110,116,68,97,116,97,0,98,116,71,101,110,101,114, -105,99,54,68,111,102,67,111,110,115,116,114,97,105,110,116,68,97,116,97, -0,98,116,71,101,110,101,114,105,99,54,68,111,102,83,112,114,105,110,103, -67,111,110,115,116,114,97,105,110,116,68,97,116,97,0,98,116,83,108,105, -100,101,114,67,111,110,115,116,114,97,105,110,116,68,97,116,97,0,83,111, -102,116,66,111,100,121,77,97,116,101,114,105,97,108,68,97,116,97,0,83, -111,102,116,66,111,100,121,78,111,100,101,68,97,116,97,0,83,111,102,116, -66,111,100,121,76,105,110,107,68,97,116,97,0,83,111,102,116,66,111,100, -121,70,97,99,101,68,97,116,97,0,83,111,102,116,66,111,100,121,84,101, -116,114,97,68,97,116,97,0,83,111,102,116,82,105,103,105,100,65,110,99, -104,111,114,68,97,116,97,0,83,111,102,116,66,111,100,121,67,111,110,102, -105,103,68,97,116,97,0,83,111,102,116,66,111,100,121,80,111,115,101,68, -97,116,97,0,83,111,102,116,66,111,100,121,67,108,117,115,116,101,114,68, -97,116,97,0,98,116,83,111,102,116,66,111,100,121,74,111,105,110,116,68, -97,116,97,0,98,116,83,111,102,116,66,111,100,121,70,108,111,97,116,68, -97,116,97,0,84,76,69,78,1,0,1,0,2,0,2,0,4,0,4,0, -4,0,4,0,8,0,0,0,12,0,36,0,8,0,16,0,32,0,48,0, -96,0,64,0,-128,0,20,0,48,0,80,0,16,0,84,0,-124,0,12,0, -52,0,52,0,20,0,64,0,4,0,4,0,8,0,4,0,32,0,28,0, -60,0,56,0,76,0,76,0,24,0,60,0,60,0,16,0,64,0,68,0, --56,1,-8,0,-32,1,-104,3,8,0,44,0,0,0,76,0,108,0,84,1, --44,0,-52,0,-12,0,84,1,-60,0,16,0,100,0,20,0,36,0,100,0, -92,0,104,0,-64,0,92,1,104,0,-92,1,83,84,82,67,61,0,0,0, -10,0,3,0,4,0,0,0,4,0,1,0,9,0,2,0,11,0,3,0, -10,0,3,0,10,0,4,0,10,0,5,0,12,0,2,0,9,0,6,0, -9,0,7,0,13,0,1,0,7,0,8,0,14,0,1,0,8,0,8,0, -15,0,1,0,13,0,9,0,16,0,1,0,14,0,9,0,17,0,2,0, -15,0,10,0,13,0,11,0,18,0,2,0,16,0,10,0,14,0,11,0, -19,0,4,0,4,0,12,0,4,0,13,0,2,0,14,0,2,0,15,0, -20,0,6,0,13,0,16,0,13,0,17,0,4,0,18,0,4,0,19,0, -4,0,20,0,0,0,21,0,21,0,6,0,14,0,16,0,14,0,17,0, -4,0,18,0,4,0,19,0,4,0,20,0,0,0,21,0,22,0,3,0, -2,0,14,0,2,0,15,0,4,0,22,0,23,0,12,0,13,0,23,0, -13,0,24,0,13,0,25,0,4,0,26,0,4,0,27,0,4,0,28,0, -4,0,29,0,20,0,30,0,22,0,31,0,19,0,32,0,4,0,33,0, -4,0,34,0,24,0,12,0,14,0,23,0,14,0,24,0,14,0,25,0, -4,0,26,0,4,0,27,0,4,0,28,0,4,0,29,0,21,0,30,0, -22,0,31,0,4,0,33,0,4,0,34,0,19,0,32,0,25,0,3,0, -0,0,35,0,4,0,36,0,0,0,37,0,26,0,5,0,25,0,38,0, -13,0,39,0,13,0,40,0,7,0,41,0,0,0,21,0,27,0,5,0, -25,0,38,0,13,0,39,0,13,0,42,0,7,0,43,0,4,0,44,0, -28,0,2,0,13,0,45,0,7,0,46,0,29,0,4,0,27,0,47,0, -28,0,48,0,4,0,49,0,0,0,37,0,30,0,1,0,4,0,50,0, -31,0,2,0,2,0,50,0,0,0,51,0,32,0,2,0,2,0,52,0, -0,0,51,0,33,0,2,0,0,0,52,0,0,0,53,0,34,0,8,0, -13,0,54,0,14,0,55,0,30,0,56,0,32,0,57,0,33,0,58,0, -31,0,59,0,4,0,60,0,4,0,61,0,35,0,4,0,34,0,62,0, -13,0,63,0,4,0,64,0,0,0,37,0,36,0,7,0,25,0,38,0, -35,0,65,0,23,0,66,0,24,0,67,0,37,0,68,0,7,0,43,0, -0,0,69,0,38,0,2,0,36,0,70,0,13,0,39,0,39,0,4,0, -17,0,71,0,25,0,72,0,4,0,73,0,7,0,74,0,40,0,4,0, -25,0,38,0,39,0,75,0,4,0,76,0,7,0,43,0,41,0,3,0, -27,0,47,0,4,0,77,0,0,0,37,0,42,0,3,0,27,0,47,0, -4,0,77,0,0,0,37,0,43,0,4,0,4,0,78,0,7,0,79,0, -7,0,80,0,7,0,81,0,37,0,14,0,4,0,82,0,4,0,83,0, -43,0,84,0,4,0,85,0,7,0,86,0,7,0,87,0,7,0,88,0, -7,0,89,0,7,0,90,0,4,0,91,0,4,0,92,0,4,0,93,0, -4,0,94,0,0,0,37,0,44,0,5,0,25,0,38,0,35,0,65,0, -13,0,39,0,7,0,43,0,4,0,95,0,45,0,5,0,27,0,47,0, -13,0,96,0,14,0,97,0,4,0,98,0,0,0,99,0,46,0,24,0, -9,0,100,0,9,0,101,0,25,0,102,0,0,0,35,0,18,0,103,0, -18,0,104,0,14,0,105,0,14,0,106,0,14,0,107,0,8,0,108,0, -8,0,109,0,8,0,110,0,8,0,111,0,8,0,112,0,8,0,113,0, -8,0,114,0,4,0,115,0,4,0,116,0,4,0,117,0,4,0,118,0, -4,0,119,0,4,0,120,0,4,0,121,0,0,0,37,0,47,0,23,0, -9,0,100,0,9,0,101,0,25,0,102,0,0,0,35,0,17,0,103,0, -17,0,104,0,13,0,105,0,13,0,106,0,13,0,107,0,7,0,108,0, -7,0,109,0,7,0,110,0,7,0,111,0,7,0,112,0,7,0,113,0, -7,0,114,0,4,0,115,0,4,0,116,0,4,0,117,0,4,0,118,0, -4,0,119,0,4,0,120,0,4,0,121,0,48,0,21,0,47,0,122,0, -15,0,123,0,13,0,124,0,13,0,125,0,13,0,126,0,13,0,127,0, -13,0,-128,0,13,0,-127,0,13,0,-126,0,13,0,-125,0,13,0,-124,0, -7,0,-123,0,7,0,-122,0,7,0,-121,0,7,0,-120,0,7,0,-119,0, -7,0,-118,0,7,0,-117,0,7,0,-116,0,7,0,-115,0,4,0,-114,0, -49,0,22,0,46,0,122,0,16,0,123,0,14,0,124,0,14,0,125,0, -14,0,126,0,14,0,127,0,14,0,-128,0,14,0,-127,0,14,0,-126,0, -14,0,-125,0,14,0,-124,0,8,0,-123,0,8,0,-122,0,8,0,-121,0, -8,0,-120,0,8,0,-119,0,8,0,-118,0,8,0,-117,0,8,0,-116,0, -8,0,-115,0,4,0,-114,0,0,0,37,0,50,0,2,0,4,0,-113,0, -4,0,-112,0,51,0,11,0,52,0,-111,0,52,0,-110,0,0,0,35,0, -4,0,-109,0,4,0,-108,0,4,0,-107,0,4,0,-106,0,7,0,-105,0, -7,0,-104,0,4,0,-103,0,0,0,-102,0,53,0,3,0,51,0,-101,0, -13,0,-100,0,13,0,-99,0,54,0,3,0,51,0,-101,0,14,0,-100,0, -14,0,-99,0,55,0,13,0,51,0,-101,0,18,0,-98,0,18,0,-97,0, -4,0,-96,0,4,0,-95,0,4,0,-94,0,7,0,-93,0,7,0,-92,0, -7,0,-91,0,7,0,-90,0,7,0,-89,0,7,0,-88,0,7,0,-87,0, -56,0,13,0,51,0,-101,0,17,0,-98,0,17,0,-97,0,4,0,-96,0, -4,0,-95,0,4,0,-94,0,7,0,-93,0,7,0,-92,0,7,0,-91,0, -7,0,-90,0,7,0,-89,0,7,0,-88,0,7,0,-87,0,57,0,11,0, -51,0,-101,0,17,0,-98,0,17,0,-97,0,7,0,-86,0,7,0,-85,0, -7,0,-84,0,7,0,-89,0,7,0,-88,0,7,0,-87,0,7,0,-83,0, -0,0,21,0,58,0,9,0,51,0,-101,0,17,0,-98,0,17,0,-97,0, -13,0,-82,0,13,0,-81,0,13,0,-80,0,13,0,-79,0,4,0,-78,0, -4,0,-77,0,59,0,5,0,58,0,-76,0,4,0,-75,0,7,0,-74,0, -7,0,-73,0,7,0,-72,0,60,0,9,0,51,0,-101,0,17,0,-98,0, -17,0,-97,0,7,0,-82,0,7,0,-81,0,7,0,-80,0,7,0,-79,0, -4,0,-78,0,4,0,-77,0,61,0,4,0,7,0,-71,0,7,0,-70,0, -7,0,-69,0,4,0,78,0,62,0,10,0,61,0,-68,0,13,0,-67,0, -13,0,-66,0,13,0,-65,0,13,0,-64,0,13,0,-63,0,7,0,-123,0, -7,0,-62,0,4,0,-61,0,4,0,53,0,63,0,4,0,61,0,-68,0, -4,0,-60,0,7,0,-59,0,4,0,-58,0,64,0,4,0,13,0,-63,0, -61,0,-68,0,4,0,-57,0,7,0,-56,0,65,0,7,0,13,0,-55,0, -61,0,-68,0,4,0,-54,0,7,0,-53,0,7,0,-52,0,7,0,-51,0, -4,0,53,0,66,0,6,0,15,0,-50,0,13,0,-52,0,13,0,-49,0, -52,0,-48,0,4,0,-47,0,7,0,-51,0,67,0,26,0,4,0,-46,0, -7,0,-45,0,7,0,-83,0,7,0,-44,0,7,0,-43,0,7,0,-42,0, -7,0,-41,0,7,0,-40,0,7,0,-39,0,7,0,-38,0,7,0,-37,0, -7,0,-36,0,7,0,-35,0,7,0,-34,0,7,0,-33,0,7,0,-32,0, -7,0,-31,0,7,0,-30,0,7,0,-29,0,7,0,-28,0,7,0,-27,0, -4,0,-26,0,4,0,-25,0,4,0,-24,0,4,0,-23,0,4,0,116,0, -68,0,12,0,15,0,-22,0,15,0,-21,0,15,0,-20,0,13,0,-19,0, -13,0,-18,0,7,0,-17,0,4,0,-16,0,4,0,-15,0,4,0,-14,0, -4,0,-13,0,7,0,-53,0,4,0,53,0,69,0,27,0,17,0,-12,0, -15,0,-11,0,15,0,-10,0,13,0,-19,0,13,0,-9,0,13,0,-8,0, -13,0,-7,0,13,0,-6,0,13,0,-5,0,4,0,-4,0,7,0,-3,0, -4,0,-2,0,4,0,-1,0,4,0,0,1,7,0,1,1,7,0,2,1, -4,0,3,1,4,0,4,1,7,0,5,1,7,0,6,1,7,0,7,1, -7,0,8,1,7,0,9,1,7,0,10,1,4,0,11,1,4,0,12,1, -4,0,13,1,70,0,12,0,9,0,14,1,9,0,15,1,13,0,16,1, -7,0,17,1,7,0,18,1,7,0,19,1,4,0,20,1,13,0,21,1, -4,0,22,1,4,0,23,1,4,0,24,1,4,0,53,0,71,0,19,0, -47,0,122,0,68,0,25,1,61,0,26,1,62,0,27,1,63,0,28,1, -64,0,29,1,65,0,30,1,66,0,31,1,69,0,32,1,70,0,33,1, -4,0,34,1,4,0,-1,0,4,0,35,1,4,0,36,1,4,0,37,1, -4,0,38,1,4,0,39,1,4,0,40,1,67,0,41,1,}; +111,77,97,112,68,97,116,97,0,98,116,83,99,97,108,101,100,84,114,105, +97,110,103,108,101,77,101,115,104,83,104,97,112,101,68,97,116,97,0,98, +116,67,111,109,112,111,117,110,100,83,104,97,112,101,67,104,105,108,100,68, +97,116,97,0,98,116,67,111,109,112,111,117,110,100,83,104,97,112,101,68, +97,116,97,0,98,116,67,121,108,105,110,100,101,114,83,104,97,112,101,68, +97,116,97,0,98,116,67,97,112,115,117,108,101,83,104,97,112,101,68,97, +116,97,0,98,116,84,114,105,97,110,103,108,101,73,110,102,111,68,97,116, +97,0,98,116,71,73,109,112,97,99,116,77,101,115,104,83,104,97,112,101, +68,97,116,97,0,98,116,67,111,110,118,101,120,72,117,108,108,83,104,97, +112,101,68,97,116,97,0,98,116,67,111,108,108,105,115,105,111,110,79,98, +106,101,99,116,68,111,117,98,108,101,68,97,116,97,0,98,116,67,111,108, +108,105,115,105,111,110,79,98,106,101,99,116,70,108,111,97,116,68,97,116, +97,0,98,116,82,105,103,105,100,66,111,100,121,70,108,111,97,116,68,97, +116,97,0,98,116,82,105,103,105,100,66,111,100,121,68,111,117,98,108,101, +68,97,116,97,0,98,116,67,111,110,115,116,114,97,105,110,116,73,110,102, +111,49,0,98,116,84,121,112,101,100,67,111,110,115,116,114,97,105,110,116, +68,97,116,97,0,98,116,82,105,103,105,100,66,111,100,121,68,97,116,97, +0,98,116,80,111,105,110,116,50,80,111,105,110,116,67,111,110,115,116,114, +97,105,110,116,70,108,111,97,116,68,97,116,97,0,98,116,80,111,105,110, +116,50,80,111,105,110,116,67,111,110,115,116,114,97,105,110,116,68,111,117, +98,108,101,68,97,116,97,0,98,116,72,105,110,103,101,67,111,110,115,116, +114,97,105,110,116,68,111,117,98,108,101,68,97,116,97,0,98,116,72,105, +110,103,101,67,111,110,115,116,114,97,105,110,116,70,108,111,97,116,68,97, +116,97,0,98,116,67,111,110,101,84,119,105,115,116,67,111,110,115,116,114, +97,105,110,116,68,97,116,97,0,98,116,71,101,110,101,114,105,99,54,68, +111,102,67,111,110,115,116,114,97,105,110,116,68,97,116,97,0,98,116,71, +101,110,101,114,105,99,54,68,111,102,83,112,114,105,110,103,67,111,110,115, +116,114,97,105,110,116,68,97,116,97,0,98,116,83,108,105,100,101,114,67, +111,110,115,116,114,97,105,110,116,68,97,116,97,0,83,111,102,116,66,111, +100,121,77,97,116,101,114,105,97,108,68,97,116,97,0,83,111,102,116,66, +111,100,121,78,111,100,101,68,97,116,97,0,83,111,102,116,66,111,100,121, +76,105,110,107,68,97,116,97,0,83,111,102,116,66,111,100,121,70,97,99, +101,68,97,116,97,0,83,111,102,116,66,111,100,121,84,101,116,114,97,68, +97,116,97,0,83,111,102,116,82,105,103,105,100,65,110,99,104,111,114,68, +97,116,97,0,83,111,102,116,66,111,100,121,67,111,110,102,105,103,68,97, +116,97,0,83,111,102,116,66,111,100,121,80,111,115,101,68,97,116,97,0, +83,111,102,116,66,111,100,121,67,108,117,115,116,101,114,68,97,116,97,0, +98,116,83,111,102,116,66,111,100,121,74,111,105,110,116,68,97,116,97,0, +98,116,83,111,102,116,66,111,100,121,70,108,111,97,116,68,97,116,97,0, +84,76,69,78,1,0,1,0,2,0,2,0,4,0,4,0,4,0,4,0, +8,0,0,0,12,0,36,0,8,0,16,0,32,0,48,0,96,0,64,0, +-128,0,20,0,48,0,80,0,16,0,84,0,-124,0,12,0,52,0,52,0, +20,0,64,0,4,0,4,0,8,0,4,0,32,0,28,0,60,0,56,0, +76,0,76,0,24,0,60,0,60,0,16,0,64,0,68,0,-56,1,-8,0, +-32,1,-104,3,8,0,52,0,0,0,84,0,116,0,92,1,-36,0,-44,0, +-4,0,92,1,-52,0,16,0,100,0,20,0,36,0,100,0,92,0,104,0, +-64,0,92,1,104,0,-92,1,83,84,82,67,61,0,0,0,10,0,3,0, +4,0,0,0,4,0,1,0,9,0,2,0,11,0,3,0,10,0,3,0, +10,0,4,0,10,0,5,0,12,0,2,0,9,0,6,0,9,0,7,0, +13,0,1,0,7,0,8,0,14,0,1,0,8,0,8,0,15,0,1,0, +13,0,9,0,16,0,1,0,14,0,9,0,17,0,2,0,15,0,10,0, +13,0,11,0,18,0,2,0,16,0,10,0,14,0,11,0,19,0,4,0, +4,0,12,0,4,0,13,0,2,0,14,0,2,0,15,0,20,0,6,0, +13,0,16,0,13,0,17,0,4,0,18,0,4,0,19,0,4,0,20,0, +0,0,21,0,21,0,6,0,14,0,16,0,14,0,17,0,4,0,18,0, +4,0,19,0,4,0,20,0,0,0,21,0,22,0,3,0,2,0,14,0, +2,0,15,0,4,0,22,0,23,0,12,0,13,0,23,0,13,0,24,0, +13,0,25,0,4,0,26,0,4,0,27,0,4,0,28,0,4,0,29,0, +20,0,30,0,22,0,31,0,19,0,32,0,4,0,33,0,4,0,34,0, +24,0,12,0,14,0,23,0,14,0,24,0,14,0,25,0,4,0,26,0, +4,0,27,0,4,0,28,0,4,0,29,0,21,0,30,0,22,0,31,0, +4,0,33,0,4,0,34,0,19,0,32,0,25,0,3,0,0,0,35,0, +4,0,36,0,0,0,37,0,26,0,5,0,25,0,38,0,13,0,39,0, +13,0,40,0,7,0,41,0,0,0,21,0,27,0,5,0,25,0,38,0, +13,0,39,0,13,0,42,0,7,0,43,0,4,0,44,0,28,0,2,0, +13,0,45,0,7,0,46,0,29,0,4,0,27,0,47,0,28,0,48,0, +4,0,49,0,0,0,37,0,30,0,1,0,4,0,50,0,31,0,2,0, +2,0,50,0,0,0,51,0,32,0,2,0,2,0,52,0,0,0,51,0, +33,0,2,0,0,0,52,0,0,0,53,0,34,0,8,0,13,0,54,0, +14,0,55,0,30,0,56,0,32,0,57,0,33,0,58,0,31,0,59,0, +4,0,60,0,4,0,61,0,35,0,4,0,34,0,62,0,13,0,63,0, +4,0,64,0,0,0,37,0,36,0,7,0,25,0,38,0,35,0,65,0, +23,0,66,0,24,0,67,0,37,0,68,0,7,0,43,0,0,0,69,0, +38,0,2,0,36,0,70,0,13,0,39,0,39,0,4,0,17,0,71,0, +25,0,72,0,4,0,73,0,7,0,74,0,40,0,4,0,25,0,38,0, +39,0,75,0,4,0,76,0,7,0,43,0,41,0,3,0,27,0,47,0, +4,0,77,0,0,0,37,0,42,0,3,0,27,0,47,0,4,0,77,0, +0,0,37,0,43,0,4,0,4,0,78,0,7,0,79,0,7,0,80,0, +7,0,81,0,37,0,14,0,4,0,82,0,4,0,83,0,43,0,84,0, +4,0,85,0,7,0,86,0,7,0,87,0,7,0,88,0,7,0,89,0, +7,0,90,0,4,0,91,0,4,0,92,0,4,0,93,0,4,0,94,0, +0,0,37,0,44,0,5,0,25,0,38,0,35,0,65,0,13,0,39,0, +7,0,43,0,4,0,95,0,45,0,5,0,27,0,47,0,13,0,96,0, +14,0,97,0,4,0,98,0,0,0,99,0,46,0,24,0,9,0,100,0, +9,0,101,0,25,0,102,0,0,0,35,0,18,0,103,0,18,0,104,0, +14,0,105,0,14,0,106,0,14,0,107,0,8,0,108,0,8,0,109,0, +8,0,110,0,8,0,111,0,8,0,112,0,8,0,113,0,8,0,114,0, +4,0,115,0,4,0,116,0,4,0,117,0,4,0,118,0,4,0,119,0, +4,0,120,0,4,0,121,0,0,0,37,0,47,0,23,0,9,0,100,0, +9,0,101,0,25,0,102,0,0,0,35,0,17,0,103,0,17,0,104,0, +13,0,105,0,13,0,106,0,13,0,107,0,7,0,108,0,7,0,109,0, +7,0,110,0,7,0,111,0,7,0,112,0,7,0,113,0,7,0,114,0, +4,0,115,0,4,0,116,0,4,0,117,0,4,0,118,0,4,0,119,0, +4,0,120,0,4,0,121,0,48,0,21,0,47,0,122,0,15,0,123,0, +13,0,124,0,13,0,125,0,13,0,126,0,13,0,127,0,13,0,-128,0, +13,0,-127,0,13,0,-126,0,13,0,-125,0,13,0,-124,0,7,0,-123,0, +7,0,-122,0,7,0,-121,0,7,0,-120,0,7,0,-119,0,7,0,-118,0, +7,0,-117,0,7,0,-116,0,7,0,-115,0,4,0,-114,0,49,0,22,0, +46,0,122,0,16,0,123,0,14,0,124,0,14,0,125,0,14,0,126,0, +14,0,127,0,14,0,-128,0,14,0,-127,0,14,0,-126,0,14,0,-125,0, +14,0,-124,0,8,0,-123,0,8,0,-122,0,8,0,-121,0,8,0,-120,0, +8,0,-119,0,8,0,-118,0,8,0,-117,0,8,0,-116,0,8,0,-115,0, +4,0,-114,0,0,0,37,0,50,0,2,0,4,0,-113,0,4,0,-112,0, +51,0,13,0,52,0,-111,0,52,0,-110,0,0,0,35,0,4,0,-109,0, +4,0,-108,0,4,0,-107,0,4,0,-106,0,7,0,-105,0,7,0,-104,0, +4,0,-103,0,4,0,-102,0,7,0,-101,0,4,0,-100,0,53,0,3,0, +51,0,-99,0,13,0,-98,0,13,0,-97,0,54,0,3,0,51,0,-99,0, +14,0,-98,0,14,0,-97,0,55,0,13,0,51,0,-99,0,18,0,-96,0, +18,0,-95,0,4,0,-94,0,4,0,-93,0,4,0,-92,0,7,0,-91,0, +7,0,-90,0,7,0,-89,0,7,0,-88,0,7,0,-87,0,7,0,-86,0, +7,0,-85,0,56,0,13,0,51,0,-99,0,17,0,-96,0,17,0,-95,0, +4,0,-94,0,4,0,-93,0,4,0,-92,0,7,0,-91,0,7,0,-90,0, +7,0,-89,0,7,0,-88,0,7,0,-87,0,7,0,-86,0,7,0,-85,0, +57,0,11,0,51,0,-99,0,17,0,-96,0,17,0,-95,0,7,0,-84,0, +7,0,-83,0,7,0,-82,0,7,0,-87,0,7,0,-86,0,7,0,-85,0, +7,0,-81,0,0,0,21,0,58,0,9,0,51,0,-99,0,17,0,-96,0, +17,0,-95,0,13,0,-80,0,13,0,-79,0,13,0,-78,0,13,0,-77,0, +4,0,-76,0,4,0,-75,0,59,0,5,0,58,0,-74,0,4,0,-73,0, +7,0,-72,0,7,0,-71,0,7,0,-70,0,60,0,9,0,51,0,-99,0, +17,0,-96,0,17,0,-95,0,7,0,-80,0,7,0,-79,0,7,0,-78,0, +7,0,-77,0,4,0,-76,0,4,0,-75,0,61,0,4,0,7,0,-69,0, +7,0,-68,0,7,0,-67,0,4,0,78,0,62,0,10,0,61,0,-66,0, +13,0,-65,0,13,0,-64,0,13,0,-63,0,13,0,-62,0,13,0,-61,0, +7,0,-123,0,7,0,-60,0,4,0,-59,0,4,0,53,0,63,0,4,0, +61,0,-66,0,4,0,-58,0,7,0,-57,0,4,0,-56,0,64,0,4,0, +13,0,-61,0,61,0,-66,0,4,0,-55,0,7,0,-54,0,65,0,7,0, +13,0,-53,0,61,0,-66,0,4,0,-52,0,7,0,-51,0,7,0,-50,0, +7,0,-49,0,4,0,53,0,66,0,6,0,15,0,-48,0,13,0,-50,0, +13,0,-47,0,52,0,-46,0,4,0,-45,0,7,0,-49,0,67,0,26,0, +4,0,-44,0,7,0,-43,0,7,0,-81,0,7,0,-42,0,7,0,-41,0, +7,0,-40,0,7,0,-39,0,7,0,-38,0,7,0,-37,0,7,0,-36,0, +7,0,-35,0,7,0,-34,0,7,0,-33,0,7,0,-32,0,7,0,-31,0, +7,0,-30,0,7,0,-29,0,7,0,-28,0,7,0,-27,0,7,0,-26,0, +7,0,-25,0,4,0,-24,0,4,0,-23,0,4,0,-22,0,4,0,-21,0, +4,0,116,0,68,0,12,0,15,0,-20,0,15,0,-19,0,15,0,-18,0, +13,0,-17,0,13,0,-16,0,7,0,-15,0,4,0,-14,0,4,0,-13,0, +4,0,-12,0,4,0,-11,0,7,0,-51,0,4,0,53,0,69,0,27,0, +17,0,-10,0,15,0,-9,0,15,0,-8,0,13,0,-17,0,13,0,-7,0, +13,0,-6,0,13,0,-5,0,13,0,-4,0,13,0,-3,0,4,0,-2,0, +7,0,-1,0,4,0,0,1,4,0,1,1,4,0,2,1,7,0,3,1, +7,0,4,1,4,0,5,1,4,0,6,1,7,0,7,1,7,0,8,1, +7,0,9,1,7,0,10,1,7,0,11,1,7,0,12,1,4,0,13,1, +4,0,14,1,4,0,15,1,70,0,12,0,9,0,16,1,9,0,17,1, +13,0,18,1,7,0,19,1,7,0,20,1,7,0,21,1,4,0,22,1, +13,0,23,1,4,0,24,1,4,0,25,1,4,0,26,1,4,0,53,0, +71,0,19,0,47,0,122,0,68,0,27,1,61,0,28,1,62,0,29,1, +63,0,30,1,64,0,31,1,65,0,32,1,66,0,33,1,69,0,34,1, +70,0,35,1,4,0,36,1,4,0,1,1,4,0,37,1,4,0,38,1, +4,0,39,1,4,0,40,1,4,0,41,1,4,0,42,1,67,0,43,1, +}; int sBulletDNAlen= sizeof(sBulletDNAstr); -unsigned char sBulletDNAstr64[]= { -83,68,78,65,78,65,77,69,42,1,0,0,109,95,115,105,122,101,0,109, + + char sBulletDNAstr64[]= { +83,68,78,65,78,65,77,69,44,1,0,0,109,95,115,105,122,101,0,109, 95,99,97,112,97,99,105,116,121,0,42,109,95,100,97,116,97,0,109,95, 99,111,108,108,105,115,105,111,110,83,104,97,112,101,115,0,109,95,99,111, 108,108,105,115,105,111,110,79,98,106,101,99,116,115,0,109,95,99,111,110, @@ -546,287 +551,291 @@ unsigned char sBulletDNAstr64[]= { 112,108,105,101,100,73,109,112,117,108,115,101,0,109,95,100,98,103,68,114, 97,119,83,105,122,101,0,109,95,100,105,115,97,98,108,101,67,111,108,108, 105,115,105,111,110,115,66,101,116,119,101,101,110,76,105,110,107,101,100,66, -111,100,105,101,115,0,109,95,112,97,100,52,91,52,93,0,109,95,116,121, -112,101,67,111,110,115,116,114,97,105,110,116,68,97,116,97,0,109,95,112, -105,118,111,116,73,110,65,0,109,95,112,105,118,111,116,73,110,66,0,109, -95,114,98,65,70,114,97,109,101,0,109,95,114,98,66,70,114,97,109,101, -0,109,95,117,115,101,82,101,102,101,114,101,110,99,101,70,114,97,109,101, -65,0,109,95,97,110,103,117,108,97,114,79,110,108,121,0,109,95,101,110, -97,98,108,101,65,110,103,117,108,97,114,77,111,116,111,114,0,109,95,109, -111,116,111,114,84,97,114,103,101,116,86,101,108,111,99,105,116,121,0,109, -95,109,97,120,77,111,116,111,114,73,109,112,117,108,115,101,0,109,95,108, -111,119,101,114,76,105,109,105,116,0,109,95,117,112,112,101,114,76,105,109, -105,116,0,109,95,108,105,109,105,116,83,111,102,116,110,101,115,115,0,109, -95,98,105,97,115,70,97,99,116,111,114,0,109,95,114,101,108,97,120,97, -116,105,111,110,70,97,99,116,111,114,0,109,95,115,119,105,110,103,83,112, -97,110,49,0,109,95,115,119,105,110,103,83,112,97,110,50,0,109,95,116, -119,105,115,116,83,112,97,110,0,109,95,100,97,109,112,105,110,103,0,109, -95,108,105,110,101,97,114,85,112,112,101,114,76,105,109,105,116,0,109,95, -108,105,110,101,97,114,76,111,119,101,114,76,105,109,105,116,0,109,95,97, -110,103,117,108,97,114,85,112,112,101,114,76,105,109,105,116,0,109,95,97, -110,103,117,108,97,114,76,111,119,101,114,76,105,109,105,116,0,109,95,117, -115,101,76,105,110,101,97,114,82,101,102,101,114,101,110,99,101,70,114,97, -109,101,65,0,109,95,117,115,101,79,102,102,115,101,116,70,111,114,67,111, -110,115,116,114,97,105,110,116,70,114,97,109,101,0,109,95,54,100,111,102, -68,97,116,97,0,109,95,115,112,114,105,110,103,69,110,97,98,108,101,100, -91,54,93,0,109,95,101,113,117,105,108,105,98,114,105,117,109,80,111,105, -110,116,91,54,93,0,109,95,115,112,114,105,110,103,83,116,105,102,102,110, -101,115,115,91,54,93,0,109,95,115,112,114,105,110,103,68,97,109,112,105, -110,103,91,54,93,0,109,95,108,105,110,101,97,114,83,116,105,102,102,110, -101,115,115,0,109,95,97,110,103,117,108,97,114,83,116,105,102,102,110,101, -115,115,0,109,95,118,111,108,117,109,101,83,116,105,102,102,110,101,115,115, -0,42,109,95,109,97,116,101,114,105,97,108,0,109,95,112,111,115,105,116, -105,111,110,0,109,95,112,114,101,118,105,111,117,115,80,111,115,105,116,105, -111,110,0,109,95,118,101,108,111,99,105,116,121,0,109,95,97,99,99,117, -109,117,108,97,116,101,100,70,111,114,99,101,0,109,95,110,111,114,109,97, -108,0,109,95,97,114,101,97,0,109,95,97,116,116,97,99,104,0,109,95, -110,111,100,101,73,110,100,105,99,101,115,91,50,93,0,109,95,114,101,115, -116,76,101,110,103,116,104,0,109,95,98,98,101,110,100,105,110,103,0,109, -95,110,111,100,101,73,110,100,105,99,101,115,91,51,93,0,109,95,114,101, -115,116,65,114,101,97,0,109,95,99,48,91,52,93,0,109,95,110,111,100, -101,73,110,100,105,99,101,115,91,52,93,0,109,95,114,101,115,116,86,111, -108,117,109,101,0,109,95,99,49,0,109,95,99,50,0,109,95,99,48,0, -109,95,108,111,99,97,108,70,114,97,109,101,0,42,109,95,114,105,103,105, -100,66,111,100,121,0,109,95,110,111,100,101,73,110,100,101,120,0,109,95, -97,101,114,111,77,111,100,101,108,0,109,95,98,97,117,109,103,97,114,116, -101,0,109,95,100,114,97,103,0,109,95,108,105,102,116,0,109,95,112,114, -101,115,115,117,114,101,0,109,95,118,111,108,117,109,101,0,109,95,100,121, -110,97,109,105,99,70,114,105,99,116,105,111,110,0,109,95,112,111,115,101, -77,97,116,99,104,0,109,95,114,105,103,105,100,67,111,110,116,97,99,116, -72,97,114,100,110,101,115,115,0,109,95,107,105,110,101,116,105,99,67,111, -110,116,97,99,116,72,97,114,100,110,101,115,115,0,109,95,115,111,102,116, -67,111,110,116,97,99,116,72,97,114,100,110,101,115,115,0,109,95,97,110, -99,104,111,114,72,97,114,100,110,101,115,115,0,109,95,115,111,102,116,82, -105,103,105,100,67,108,117,115,116,101,114,72,97,114,100,110,101,115,115,0, -109,95,115,111,102,116,75,105,110,101,116,105,99,67,108,117,115,116,101,114, -72,97,114,100,110,101,115,115,0,109,95,115,111,102,116,83,111,102,116,67, -108,117,115,116,101,114,72,97,114,100,110,101,115,115,0,109,95,115,111,102, -116,82,105,103,105,100,67,108,117,115,116,101,114,73,109,112,117,108,115,101, -83,112,108,105,116,0,109,95,115,111,102,116,75,105,110,101,116,105,99,67, -108,117,115,116,101,114,73,109,112,117,108,115,101,83,112,108,105,116,0,109, -95,115,111,102,116,83,111,102,116,67,108,117,115,116,101,114,73,109,112,117, -108,115,101,83,112,108,105,116,0,109,95,109,97,120,86,111,108,117,109,101, -0,109,95,116,105,109,101,83,99,97,108,101,0,109,95,118,101,108,111,99, -105,116,121,73,116,101,114,97,116,105,111,110,115,0,109,95,112,111,115,105, -116,105,111,110,73,116,101,114,97,116,105,111,110,115,0,109,95,100,114,105, -102,116,73,116,101,114,97,116,105,111,110,115,0,109,95,99,108,117,115,116, -101,114,73,116,101,114,97,116,105,111,110,115,0,109,95,114,111,116,0,109, -95,115,99,97,108,101,0,109,95,97,113,113,0,109,95,99,111,109,0,42, -109,95,112,111,115,105,116,105,111,110,115,0,42,109,95,119,101,105,103,104, -116,115,0,109,95,110,117,109,80,111,115,105,116,105,111,110,115,0,109,95, -110,117,109,87,101,105,103,116,115,0,109,95,98,118,111,108,117,109,101,0, -109,95,98,102,114,97,109,101,0,109,95,102,114,97,109,101,120,102,111,114, -109,0,109,95,108,111,99,105,105,0,109,95,105,110,118,119,105,0,109,95, -118,105,109,112,117,108,115,101,115,91,50,93,0,109,95,100,105,109,112,117, -108,115,101,115,91,50,93,0,109,95,108,118,0,109,95,97,118,0,42,109, -95,102,114,97,109,101,114,101,102,115,0,42,109,95,110,111,100,101,73,110, -100,105,99,101,115,0,42,109,95,109,97,115,115,101,115,0,109,95,110,117, -109,70,114,97,109,101,82,101,102,115,0,109,95,110,117,109,78,111,100,101, -115,0,109,95,110,117,109,77,97,115,115,101,115,0,109,95,105,100,109,97, -115,115,0,109,95,105,109,97,115,115,0,109,95,110,118,105,109,112,117,108, -115,101,115,0,109,95,110,100,105,109,112,117,108,115,101,115,0,109,95,110, -100,97,109,112,105,110,103,0,109,95,108,100,97,109,112,105,110,103,0,109, -95,97,100,97,109,112,105,110,103,0,109,95,109,97,116,99,104,105,110,103, -0,109,95,109,97,120,83,101,108,102,67,111,108,108,105,115,105,111,110,73, -109,112,117,108,115,101,0,109,95,115,101,108,102,67,111,108,108,105,115,105, -111,110,73,109,112,117,108,115,101,70,97,99,116,111,114,0,109,95,99,111, -110,116,97,105,110,115,65,110,99,104,111,114,0,109,95,99,111,108,108,105, -100,101,0,109,95,99,108,117,115,116,101,114,73,110,100,101,120,0,42,109, -95,98,111,100,121,65,0,42,109,95,98,111,100,121,66,0,109,95,114,101, -102,115,91,50,93,0,109,95,99,102,109,0,109,95,101,114,112,0,109,95, -115,112,108,105,116,0,109,95,100,101,108,101,116,101,0,109,95,114,101,108, -80,111,115,105,116,105,111,110,91,50,93,0,109,95,98,111,100,121,65,116, -121,112,101,0,109,95,98,111,100,121,66,116,121,112,101,0,109,95,106,111, -105,110,116,84,121,112,101,0,42,109,95,112,111,115,101,0,42,42,109,95, -109,97,116,101,114,105,97,108,115,0,42,109,95,110,111,100,101,115,0,42, -109,95,108,105,110,107,115,0,42,109,95,102,97,99,101,115,0,42,109,95, -116,101,116,114,97,104,101,100,114,97,0,42,109,95,97,110,99,104,111,114, -115,0,42,109,95,99,108,117,115,116,101,114,115,0,42,109,95,106,111,105, -110,116,115,0,109,95,110,117,109,77,97,116,101,114,105,97,108,115,0,109, -95,110,117,109,76,105,110,107,115,0,109,95,110,117,109,70,97,99,101,115, -0,109,95,110,117,109,84,101,116,114,97,104,101,100,114,97,0,109,95,110, -117,109,65,110,99,104,111,114,115,0,109,95,110,117,109,67,108,117,115,116, -101,114,115,0,109,95,110,117,109,74,111,105,110,116,115,0,109,95,99,111, -110,102,105,103,0,0,0,0,84,89,80,69,72,0,0,0,99,104,97,114, -0,117,99,104,97,114,0,115,104,111,114,116,0,117,115,104,111,114,116,0, -105,110,116,0,108,111,110,103,0,117,108,111,110,103,0,102,108,111,97,116, -0,100,111,117,98,108,101,0,118,111,105,100,0,80,111,105,110,116,101,114, -65,114,114,97,121,0,98,116,80,104,121,115,105,99,115,83,121,115,116,101, -109,0,76,105,115,116,66,97,115,101,0,98,116,86,101,99,116,111,114,51, -70,108,111,97,116,68,97,116,97,0,98,116,86,101,99,116,111,114,51,68, -111,117,98,108,101,68,97,116,97,0,98,116,77,97,116,114,105,120,51,120, -51,70,108,111,97,116,68,97,116,97,0,98,116,77,97,116,114,105,120,51, -120,51,68,111,117,98,108,101,68,97,116,97,0,98,116,84,114,97,110,115, -102,111,114,109,70,108,111,97,116,68,97,116,97,0,98,116,84,114,97,110, -115,102,111,114,109,68,111,117,98,108,101,68,97,116,97,0,98,116,66,118, -104,83,117,98,116,114,101,101,73,110,102,111,68,97,116,97,0,98,116,79, -112,116,105,109,105,122,101,100,66,118,104,78,111,100,101,70,108,111,97,116, -68,97,116,97,0,98,116,79,112,116,105,109,105,122,101,100,66,118,104,78, -111,100,101,68,111,117,98,108,101,68,97,116,97,0,98,116,81,117,97,110, -116,105,122,101,100,66,118,104,78,111,100,101,68,97,116,97,0,98,116,81, -117,97,110,116,105,122,101,100,66,118,104,70,108,111,97,116,68,97,116,97, -0,98,116,81,117,97,110,116,105,122,101,100,66,118,104,68,111,117,98,108, -101,68,97,116,97,0,98,116,67,111,108,108,105,115,105,111,110,83,104,97, -112,101,68,97,116,97,0,98,116,83,116,97,116,105,99,80,108,97,110,101, -83,104,97,112,101,68,97,116,97,0,98,116,67,111,110,118,101,120,73,110, -116,101,114,110,97,108,83,104,97,112,101,68,97,116,97,0,98,116,80,111, -115,105,116,105,111,110,65,110,100,82,97,100,105,117,115,0,98,116,77,117, -108,116,105,83,112,104,101,114,101,83,104,97,112,101,68,97,116,97,0,98, -116,73,110,116,73,110,100,101,120,68,97,116,97,0,98,116,83,104,111,114, -116,73,110,116,73,110,100,101,120,68,97,116,97,0,98,116,83,104,111,114, -116,73,110,116,73,110,100,101,120,84,114,105,112,108,101,116,68,97,116,97, -0,98,116,67,104,97,114,73,110,100,101,120,84,114,105,112,108,101,116,68, -97,116,97,0,98,116,77,101,115,104,80,97,114,116,68,97,116,97,0,98, -116,83,116,114,105,100,105,110,103,77,101,115,104,73,110,116,101,114,102,97, -99,101,68,97,116,97,0,98,116,84,114,105,97,110,103,108,101,77,101,115, -104,83,104,97,112,101,68,97,116,97,0,98,116,84,114,105,97,110,103,108, -101,73,110,102,111,77,97,112,68,97,116,97,0,98,116,83,99,97,108,101, -100,84,114,105,97,110,103,108,101,77,101,115,104,83,104,97,112,101,68,97, -116,97,0,98,116,67,111,109,112,111,117,110,100,83,104,97,112,101,67,104, -105,108,100,68,97,116,97,0,98,116,67,111,109,112,111,117,110,100,83,104, -97,112,101,68,97,116,97,0,98,116,67,121,108,105,110,100,101,114,83,104, -97,112,101,68,97,116,97,0,98,116,67,97,112,115,117,108,101,83,104,97, +111,100,105,101,115,0,109,95,111,118,101,114,114,105,100,101,78,117,109,83, +111,108,118,101,114,73,116,101,114,97,116,105,111,110,115,0,109,95,98,114, +101,97,107,105,110,103,73,109,112,117,108,115,101,84,104,114,101,115,104,111, +108,100,0,109,95,105,115,69,110,97,98,108,101,100,0,109,95,116,121,112, +101,67,111,110,115,116,114,97,105,110,116,68,97,116,97,0,109,95,112,105, +118,111,116,73,110,65,0,109,95,112,105,118,111,116,73,110,66,0,109,95, +114,98,65,70,114,97,109,101,0,109,95,114,98,66,70,114,97,109,101,0, +109,95,117,115,101,82,101,102,101,114,101,110,99,101,70,114,97,109,101,65, +0,109,95,97,110,103,117,108,97,114,79,110,108,121,0,109,95,101,110,97, +98,108,101,65,110,103,117,108,97,114,77,111,116,111,114,0,109,95,109,111, +116,111,114,84,97,114,103,101,116,86,101,108,111,99,105,116,121,0,109,95, +109,97,120,77,111,116,111,114,73,109,112,117,108,115,101,0,109,95,108,111, +119,101,114,76,105,109,105,116,0,109,95,117,112,112,101,114,76,105,109,105, +116,0,109,95,108,105,109,105,116,83,111,102,116,110,101,115,115,0,109,95, +98,105,97,115,70,97,99,116,111,114,0,109,95,114,101,108,97,120,97,116, +105,111,110,70,97,99,116,111,114,0,109,95,115,119,105,110,103,83,112,97, +110,49,0,109,95,115,119,105,110,103,83,112,97,110,50,0,109,95,116,119, +105,115,116,83,112,97,110,0,109,95,100,97,109,112,105,110,103,0,109,95, +108,105,110,101,97,114,85,112,112,101,114,76,105,109,105,116,0,109,95,108, +105,110,101,97,114,76,111,119,101,114,76,105,109,105,116,0,109,95,97,110, +103,117,108,97,114,85,112,112,101,114,76,105,109,105,116,0,109,95,97,110, +103,117,108,97,114,76,111,119,101,114,76,105,109,105,116,0,109,95,117,115, +101,76,105,110,101,97,114,82,101,102,101,114,101,110,99,101,70,114,97,109, +101,65,0,109,95,117,115,101,79,102,102,115,101,116,70,111,114,67,111,110, +115,116,114,97,105,110,116,70,114,97,109,101,0,109,95,54,100,111,102,68, +97,116,97,0,109,95,115,112,114,105,110,103,69,110,97,98,108,101,100,91, +54,93,0,109,95,101,113,117,105,108,105,98,114,105,117,109,80,111,105,110, +116,91,54,93,0,109,95,115,112,114,105,110,103,83,116,105,102,102,110,101, +115,115,91,54,93,0,109,95,115,112,114,105,110,103,68,97,109,112,105,110, +103,91,54,93,0,109,95,108,105,110,101,97,114,83,116,105,102,102,110,101, +115,115,0,109,95,97,110,103,117,108,97,114,83,116,105,102,102,110,101,115, +115,0,109,95,118,111,108,117,109,101,83,116,105,102,102,110,101,115,115,0, +42,109,95,109,97,116,101,114,105,97,108,0,109,95,112,111,115,105,116,105, +111,110,0,109,95,112,114,101,118,105,111,117,115,80,111,115,105,116,105,111, +110,0,109,95,118,101,108,111,99,105,116,121,0,109,95,97,99,99,117,109, +117,108,97,116,101,100,70,111,114,99,101,0,109,95,110,111,114,109,97,108, +0,109,95,97,114,101,97,0,109,95,97,116,116,97,99,104,0,109,95,110, +111,100,101,73,110,100,105,99,101,115,91,50,93,0,109,95,114,101,115,116, +76,101,110,103,116,104,0,109,95,98,98,101,110,100,105,110,103,0,109,95, +110,111,100,101,73,110,100,105,99,101,115,91,51,93,0,109,95,114,101,115, +116,65,114,101,97,0,109,95,99,48,91,52,93,0,109,95,110,111,100,101, +73,110,100,105,99,101,115,91,52,93,0,109,95,114,101,115,116,86,111,108, +117,109,101,0,109,95,99,49,0,109,95,99,50,0,109,95,99,48,0,109, +95,108,111,99,97,108,70,114,97,109,101,0,42,109,95,114,105,103,105,100, +66,111,100,121,0,109,95,110,111,100,101,73,110,100,101,120,0,109,95,97, +101,114,111,77,111,100,101,108,0,109,95,98,97,117,109,103,97,114,116,101, +0,109,95,100,114,97,103,0,109,95,108,105,102,116,0,109,95,112,114,101, +115,115,117,114,101,0,109,95,118,111,108,117,109,101,0,109,95,100,121,110, +97,109,105,99,70,114,105,99,116,105,111,110,0,109,95,112,111,115,101,77, +97,116,99,104,0,109,95,114,105,103,105,100,67,111,110,116,97,99,116,72, +97,114,100,110,101,115,115,0,109,95,107,105,110,101,116,105,99,67,111,110, +116,97,99,116,72,97,114,100,110,101,115,115,0,109,95,115,111,102,116,67, +111,110,116,97,99,116,72,97,114,100,110,101,115,115,0,109,95,97,110,99, +104,111,114,72,97,114,100,110,101,115,115,0,109,95,115,111,102,116,82,105, +103,105,100,67,108,117,115,116,101,114,72,97,114,100,110,101,115,115,0,109, +95,115,111,102,116,75,105,110,101,116,105,99,67,108,117,115,116,101,114,72, +97,114,100,110,101,115,115,0,109,95,115,111,102,116,83,111,102,116,67,108, +117,115,116,101,114,72,97,114,100,110,101,115,115,0,109,95,115,111,102,116, +82,105,103,105,100,67,108,117,115,116,101,114,73,109,112,117,108,115,101,83, +112,108,105,116,0,109,95,115,111,102,116,75,105,110,101,116,105,99,67,108, +117,115,116,101,114,73,109,112,117,108,115,101,83,112,108,105,116,0,109,95, +115,111,102,116,83,111,102,116,67,108,117,115,116,101,114,73,109,112,117,108, +115,101,83,112,108,105,116,0,109,95,109,97,120,86,111,108,117,109,101,0, +109,95,116,105,109,101,83,99,97,108,101,0,109,95,118,101,108,111,99,105, +116,121,73,116,101,114,97,116,105,111,110,115,0,109,95,112,111,115,105,116, +105,111,110,73,116,101,114,97,116,105,111,110,115,0,109,95,100,114,105,102, +116,73,116,101,114,97,116,105,111,110,115,0,109,95,99,108,117,115,116,101, +114,73,116,101,114,97,116,105,111,110,115,0,109,95,114,111,116,0,109,95, +115,99,97,108,101,0,109,95,97,113,113,0,109,95,99,111,109,0,42,109, +95,112,111,115,105,116,105,111,110,115,0,42,109,95,119,101,105,103,104,116, +115,0,109,95,110,117,109,80,111,115,105,116,105,111,110,115,0,109,95,110, +117,109,87,101,105,103,116,115,0,109,95,98,118,111,108,117,109,101,0,109, +95,98,102,114,97,109,101,0,109,95,102,114,97,109,101,120,102,111,114,109, +0,109,95,108,111,99,105,105,0,109,95,105,110,118,119,105,0,109,95,118, +105,109,112,117,108,115,101,115,91,50,93,0,109,95,100,105,109,112,117,108, +115,101,115,91,50,93,0,109,95,108,118,0,109,95,97,118,0,42,109,95, +102,114,97,109,101,114,101,102,115,0,42,109,95,110,111,100,101,73,110,100, +105,99,101,115,0,42,109,95,109,97,115,115,101,115,0,109,95,110,117,109, +70,114,97,109,101,82,101,102,115,0,109,95,110,117,109,78,111,100,101,115, +0,109,95,110,117,109,77,97,115,115,101,115,0,109,95,105,100,109,97,115, +115,0,109,95,105,109,97,115,115,0,109,95,110,118,105,109,112,117,108,115, +101,115,0,109,95,110,100,105,109,112,117,108,115,101,115,0,109,95,110,100, +97,109,112,105,110,103,0,109,95,108,100,97,109,112,105,110,103,0,109,95, +97,100,97,109,112,105,110,103,0,109,95,109,97,116,99,104,105,110,103,0, +109,95,109,97,120,83,101,108,102,67,111,108,108,105,115,105,111,110,73,109, +112,117,108,115,101,0,109,95,115,101,108,102,67,111,108,108,105,115,105,111, +110,73,109,112,117,108,115,101,70,97,99,116,111,114,0,109,95,99,111,110, +116,97,105,110,115,65,110,99,104,111,114,0,109,95,99,111,108,108,105,100, +101,0,109,95,99,108,117,115,116,101,114,73,110,100,101,120,0,42,109,95, +98,111,100,121,65,0,42,109,95,98,111,100,121,66,0,109,95,114,101,102, +115,91,50,93,0,109,95,99,102,109,0,109,95,101,114,112,0,109,95,115, +112,108,105,116,0,109,95,100,101,108,101,116,101,0,109,95,114,101,108,80, +111,115,105,116,105,111,110,91,50,93,0,109,95,98,111,100,121,65,116,121, +112,101,0,109,95,98,111,100,121,66,116,121,112,101,0,109,95,106,111,105, +110,116,84,121,112,101,0,42,109,95,112,111,115,101,0,42,42,109,95,109, +97,116,101,114,105,97,108,115,0,42,109,95,110,111,100,101,115,0,42,109, +95,108,105,110,107,115,0,42,109,95,102,97,99,101,115,0,42,109,95,116, +101,116,114,97,104,101,100,114,97,0,42,109,95,97,110,99,104,111,114,115, +0,42,109,95,99,108,117,115,116,101,114,115,0,42,109,95,106,111,105,110, +116,115,0,109,95,110,117,109,77,97,116,101,114,105,97,108,115,0,109,95, +110,117,109,76,105,110,107,115,0,109,95,110,117,109,70,97,99,101,115,0, +109,95,110,117,109,84,101,116,114,97,104,101,100,114,97,0,109,95,110,117, +109,65,110,99,104,111,114,115,0,109,95,110,117,109,67,108,117,115,116,101, +114,115,0,109,95,110,117,109,74,111,105,110,116,115,0,109,95,99,111,110, +102,105,103,0,84,89,80,69,72,0,0,0,99,104,97,114,0,117,99,104, +97,114,0,115,104,111,114,116,0,117,115,104,111,114,116,0,105,110,116,0, +108,111,110,103,0,117,108,111,110,103,0,102,108,111,97,116,0,100,111,117, +98,108,101,0,118,111,105,100,0,80,111,105,110,116,101,114,65,114,114,97, +121,0,98,116,80,104,121,115,105,99,115,83,121,115,116,101,109,0,76,105, +115,116,66,97,115,101,0,98,116,86,101,99,116,111,114,51,70,108,111,97, +116,68,97,116,97,0,98,116,86,101,99,116,111,114,51,68,111,117,98,108, +101,68,97,116,97,0,98,116,77,97,116,114,105,120,51,120,51,70,108,111, +97,116,68,97,116,97,0,98,116,77,97,116,114,105,120,51,120,51,68,111, +117,98,108,101,68,97,116,97,0,98,116,84,114,97,110,115,102,111,114,109, +70,108,111,97,116,68,97,116,97,0,98,116,84,114,97,110,115,102,111,114, +109,68,111,117,98,108,101,68,97,116,97,0,98,116,66,118,104,83,117,98, +116,114,101,101,73,110,102,111,68,97,116,97,0,98,116,79,112,116,105,109, +105,122,101,100,66,118,104,78,111,100,101,70,108,111,97,116,68,97,116,97, +0,98,116,79,112,116,105,109,105,122,101,100,66,118,104,78,111,100,101,68, +111,117,98,108,101,68,97,116,97,0,98,116,81,117,97,110,116,105,122,101, +100,66,118,104,78,111,100,101,68,97,116,97,0,98,116,81,117,97,110,116, +105,122,101,100,66,118,104,70,108,111,97,116,68,97,116,97,0,98,116,81, +117,97,110,116,105,122,101,100,66,118,104,68,111,117,98,108,101,68,97,116, +97,0,98,116,67,111,108,108,105,115,105,111,110,83,104,97,112,101,68,97, +116,97,0,98,116,83,116,97,116,105,99,80,108,97,110,101,83,104,97,112, +101,68,97,116,97,0,98,116,67,111,110,118,101,120,73,110,116,101,114,110, +97,108,83,104,97,112,101,68,97,116,97,0,98,116,80,111,115,105,116,105, +111,110,65,110,100,82,97,100,105,117,115,0,98,116,77,117,108,116,105,83, +112,104,101,114,101,83,104,97,112,101,68,97,116,97,0,98,116,73,110,116, +73,110,100,101,120,68,97,116,97,0,98,116,83,104,111,114,116,73,110,116, +73,110,100,101,120,68,97,116,97,0,98,116,83,104,111,114,116,73,110,116, +73,110,100,101,120,84,114,105,112,108,101,116,68,97,116,97,0,98,116,67, +104,97,114,73,110,100,101,120,84,114,105,112,108,101,116,68,97,116,97,0, +98,116,77,101,115,104,80,97,114,116,68,97,116,97,0,98,116,83,116,114, +105,100,105,110,103,77,101,115,104,73,110,116,101,114,102,97,99,101,68,97, +116,97,0,98,116,84,114,105,97,110,103,108,101,77,101,115,104,83,104,97, 112,101,68,97,116,97,0,98,116,84,114,105,97,110,103,108,101,73,110,102, -111,68,97,116,97,0,98,116,71,73,109,112,97,99,116,77,101,115,104,83, -104,97,112,101,68,97,116,97,0,98,116,67,111,110,118,101,120,72,117,108, -108,83,104,97,112,101,68,97,116,97,0,98,116,67,111,108,108,105,115,105, -111,110,79,98,106,101,99,116,68,111,117,98,108,101,68,97,116,97,0,98, -116,67,111,108,108,105,115,105,111,110,79,98,106,101,99,116,70,108,111,97, -116,68,97,116,97,0,98,116,82,105,103,105,100,66,111,100,121,70,108,111, -97,116,68,97,116,97,0,98,116,82,105,103,105,100,66,111,100,121,68,111, -117,98,108,101,68,97,116,97,0,98,116,67,111,110,115,116,114,97,105,110, -116,73,110,102,111,49,0,98,116,84,121,112,101,100,67,111,110,115,116,114, -97,105,110,116,68,97,116,97,0,98,116,82,105,103,105,100,66,111,100,121, -68,97,116,97,0,98,116,80,111,105,110,116,50,80,111,105,110,116,67,111, -110,115,116,114,97,105,110,116,70,108,111,97,116,68,97,116,97,0,98,116, -80,111,105,110,116,50,80,111,105,110,116,67,111,110,115,116,114,97,105,110, -116,68,111,117,98,108,101,68,97,116,97,0,98,116,72,105,110,103,101,67, -111,110,115,116,114,97,105,110,116,68,111,117,98,108,101,68,97,116,97,0, -98,116,72,105,110,103,101,67,111,110,115,116,114,97,105,110,116,70,108,111, -97,116,68,97,116,97,0,98,116,67,111,110,101,84,119,105,115,116,67,111, -110,115,116,114,97,105,110,116,68,97,116,97,0,98,116,71,101,110,101,114, -105,99,54,68,111,102,67,111,110,115,116,114,97,105,110,116,68,97,116,97, -0,98,116,71,101,110,101,114,105,99,54,68,111,102,83,112,114,105,110,103, -67,111,110,115,116,114,97,105,110,116,68,97,116,97,0,98,116,83,108,105, -100,101,114,67,111,110,115,116,114,97,105,110,116,68,97,116,97,0,83,111, -102,116,66,111,100,121,77,97,116,101,114,105,97,108,68,97,116,97,0,83, -111,102,116,66,111,100,121,78,111,100,101,68,97,116,97,0,83,111,102,116, -66,111,100,121,76,105,110,107,68,97,116,97,0,83,111,102,116,66,111,100, -121,70,97,99,101,68,97,116,97,0,83,111,102,116,66,111,100,121,84,101, -116,114,97,68,97,116,97,0,83,111,102,116,82,105,103,105,100,65,110,99, -104,111,114,68,97,116,97,0,83,111,102,116,66,111,100,121,67,111,110,102, -105,103,68,97,116,97,0,83,111,102,116,66,111,100,121,80,111,115,101,68, -97,116,97,0,83,111,102,116,66,111,100,121,67,108,117,115,116,101,114,68, -97,116,97,0,98,116,83,111,102,116,66,111,100,121,74,111,105,110,116,68, -97,116,97,0,98,116,83,111,102,116,66,111,100,121,70,108,111,97,116,68, -97,116,97,0,84,76,69,78,1,0,1,0,2,0,2,0,4,0,4,0, -4,0,4,0,8,0,0,0,16,0,48,0,16,0,16,0,32,0,48,0, -96,0,64,0,-128,0,20,0,48,0,80,0,16,0,96,0,-112,0,16,0, -56,0,56,0,20,0,72,0,4,0,4,0,8,0,4,0,56,0,32,0, -80,0,72,0,96,0,80,0,32,0,64,0,64,0,16,0,72,0,80,0, --40,1,8,1,-16,1,-88,3,8,0,56,0,0,0,88,0,120,0,96,1, --32,0,-40,0,0,1,96,1,-48,0,16,0,104,0,24,0,40,0,104,0, -96,0,104,0,-56,0,104,1,112,0,-40,1,83,84,82,67,61,0,0,0, -10,0,3,0,4,0,0,0,4,0,1,0,9,0,2,0,11,0,3,0, -10,0,3,0,10,0,4,0,10,0,5,0,12,0,2,0,9,0,6,0, -9,0,7,0,13,0,1,0,7,0,8,0,14,0,1,0,8,0,8,0, -15,0,1,0,13,0,9,0,16,0,1,0,14,0,9,0,17,0,2,0, -15,0,10,0,13,0,11,0,18,0,2,0,16,0,10,0,14,0,11,0, -19,0,4,0,4,0,12,0,4,0,13,0,2,0,14,0,2,0,15,0, -20,0,6,0,13,0,16,0,13,0,17,0,4,0,18,0,4,0,19,0, -4,0,20,0,0,0,21,0,21,0,6,0,14,0,16,0,14,0,17,0, -4,0,18,0,4,0,19,0,4,0,20,0,0,0,21,0,22,0,3,0, -2,0,14,0,2,0,15,0,4,0,22,0,23,0,12,0,13,0,23,0, -13,0,24,0,13,0,25,0,4,0,26,0,4,0,27,0,4,0,28,0, -4,0,29,0,20,0,30,0,22,0,31,0,19,0,32,0,4,0,33,0, -4,0,34,0,24,0,12,0,14,0,23,0,14,0,24,0,14,0,25,0, -4,0,26,0,4,0,27,0,4,0,28,0,4,0,29,0,21,0,30,0, -22,0,31,0,4,0,33,0,4,0,34,0,19,0,32,0,25,0,3,0, -0,0,35,0,4,0,36,0,0,0,37,0,26,0,5,0,25,0,38,0, -13,0,39,0,13,0,40,0,7,0,41,0,0,0,21,0,27,0,5,0, -25,0,38,0,13,0,39,0,13,0,42,0,7,0,43,0,4,0,44,0, -28,0,2,0,13,0,45,0,7,0,46,0,29,0,4,0,27,0,47,0, -28,0,48,0,4,0,49,0,0,0,37,0,30,0,1,0,4,0,50,0, -31,0,2,0,2,0,50,0,0,0,51,0,32,0,2,0,2,0,52,0, -0,0,51,0,33,0,2,0,0,0,52,0,0,0,53,0,34,0,8,0, -13,0,54,0,14,0,55,0,30,0,56,0,32,0,57,0,33,0,58,0, -31,0,59,0,4,0,60,0,4,0,61,0,35,0,4,0,34,0,62,0, -13,0,63,0,4,0,64,0,0,0,37,0,36,0,7,0,25,0,38,0, -35,0,65,0,23,0,66,0,24,0,67,0,37,0,68,0,7,0,43,0, -0,0,69,0,38,0,2,0,36,0,70,0,13,0,39,0,39,0,4,0, -17,0,71,0,25,0,72,0,4,0,73,0,7,0,74,0,40,0,4,0, -25,0,38,0,39,0,75,0,4,0,76,0,7,0,43,0,41,0,3,0, -27,0,47,0,4,0,77,0,0,0,37,0,42,0,3,0,27,0,47,0, -4,0,77,0,0,0,37,0,43,0,4,0,4,0,78,0,7,0,79,0, -7,0,80,0,7,0,81,0,37,0,14,0,4,0,82,0,4,0,83,0, -43,0,84,0,4,0,85,0,7,0,86,0,7,0,87,0,7,0,88,0, -7,0,89,0,7,0,90,0,4,0,91,0,4,0,92,0,4,0,93,0, -4,0,94,0,0,0,37,0,44,0,5,0,25,0,38,0,35,0,65,0, -13,0,39,0,7,0,43,0,4,0,95,0,45,0,5,0,27,0,47,0, -13,0,96,0,14,0,97,0,4,0,98,0,0,0,99,0,46,0,24,0, -9,0,100,0,9,0,101,0,25,0,102,0,0,0,35,0,18,0,103,0, -18,0,104,0,14,0,105,0,14,0,106,0,14,0,107,0,8,0,108,0, -8,0,109,0,8,0,110,0,8,0,111,0,8,0,112,0,8,0,113,0, -8,0,114,0,4,0,115,0,4,0,116,0,4,0,117,0,4,0,118,0, -4,0,119,0,4,0,120,0,4,0,121,0,0,0,37,0,47,0,23,0, -9,0,100,0,9,0,101,0,25,0,102,0,0,0,35,0,17,0,103,0, -17,0,104,0,13,0,105,0,13,0,106,0,13,0,107,0,7,0,108,0, -7,0,109,0,7,0,110,0,7,0,111,0,7,0,112,0,7,0,113,0, -7,0,114,0,4,0,115,0,4,0,116,0,4,0,117,0,4,0,118,0, -4,0,119,0,4,0,120,0,4,0,121,0,48,0,21,0,47,0,122,0, -15,0,123,0,13,0,124,0,13,0,125,0,13,0,126,0,13,0,127,0, -13,0,-128,0,13,0,-127,0,13,0,-126,0,13,0,-125,0,13,0,-124,0, -7,0,-123,0,7,0,-122,0,7,0,-121,0,7,0,-120,0,7,0,-119,0, -7,0,-118,0,7,0,-117,0,7,0,-116,0,7,0,-115,0,4,0,-114,0, -49,0,22,0,46,0,122,0,16,0,123,0,14,0,124,0,14,0,125,0, -14,0,126,0,14,0,127,0,14,0,-128,0,14,0,-127,0,14,0,-126,0, -14,0,-125,0,14,0,-124,0,8,0,-123,0,8,0,-122,0,8,0,-121,0, -8,0,-120,0,8,0,-119,0,8,0,-118,0,8,0,-117,0,8,0,-116,0, -8,0,-115,0,4,0,-114,0,0,0,37,0,50,0,2,0,4,0,-113,0, -4,0,-112,0,51,0,11,0,52,0,-111,0,52,0,-110,0,0,0,35,0, -4,0,-109,0,4,0,-108,0,4,0,-107,0,4,0,-106,0,7,0,-105,0, -7,0,-104,0,4,0,-103,0,0,0,-102,0,53,0,3,0,51,0,-101,0, -13,0,-100,0,13,0,-99,0,54,0,3,0,51,0,-101,0,14,0,-100,0, -14,0,-99,0,55,0,13,0,51,0,-101,0,18,0,-98,0,18,0,-97,0, -4,0,-96,0,4,0,-95,0,4,0,-94,0,7,0,-93,0,7,0,-92,0, -7,0,-91,0,7,0,-90,0,7,0,-89,0,7,0,-88,0,7,0,-87,0, -56,0,13,0,51,0,-101,0,17,0,-98,0,17,0,-97,0,4,0,-96,0, -4,0,-95,0,4,0,-94,0,7,0,-93,0,7,0,-92,0,7,0,-91,0, -7,0,-90,0,7,0,-89,0,7,0,-88,0,7,0,-87,0,57,0,11,0, -51,0,-101,0,17,0,-98,0,17,0,-97,0,7,0,-86,0,7,0,-85,0, -7,0,-84,0,7,0,-89,0,7,0,-88,0,7,0,-87,0,7,0,-83,0, -0,0,21,0,58,0,9,0,51,0,-101,0,17,0,-98,0,17,0,-97,0, -13,0,-82,0,13,0,-81,0,13,0,-80,0,13,0,-79,0,4,0,-78,0, -4,0,-77,0,59,0,5,0,58,0,-76,0,4,0,-75,0,7,0,-74,0, -7,0,-73,0,7,0,-72,0,60,0,9,0,51,0,-101,0,17,0,-98,0, -17,0,-97,0,7,0,-82,0,7,0,-81,0,7,0,-80,0,7,0,-79,0, -4,0,-78,0,4,0,-77,0,61,0,4,0,7,0,-71,0,7,0,-70,0, -7,0,-69,0,4,0,78,0,62,0,10,0,61,0,-68,0,13,0,-67,0, -13,0,-66,0,13,0,-65,0,13,0,-64,0,13,0,-63,0,7,0,-123,0, -7,0,-62,0,4,0,-61,0,4,0,53,0,63,0,4,0,61,0,-68,0, -4,0,-60,0,7,0,-59,0,4,0,-58,0,64,0,4,0,13,0,-63,0, -61,0,-68,0,4,0,-57,0,7,0,-56,0,65,0,7,0,13,0,-55,0, -61,0,-68,0,4,0,-54,0,7,0,-53,0,7,0,-52,0,7,0,-51,0, -4,0,53,0,66,0,6,0,15,0,-50,0,13,0,-52,0,13,0,-49,0, -52,0,-48,0,4,0,-47,0,7,0,-51,0,67,0,26,0,4,0,-46,0, -7,0,-45,0,7,0,-83,0,7,0,-44,0,7,0,-43,0,7,0,-42,0, -7,0,-41,0,7,0,-40,0,7,0,-39,0,7,0,-38,0,7,0,-37,0, -7,0,-36,0,7,0,-35,0,7,0,-34,0,7,0,-33,0,7,0,-32,0, -7,0,-31,0,7,0,-30,0,7,0,-29,0,7,0,-28,0,7,0,-27,0, -4,0,-26,0,4,0,-25,0,4,0,-24,0,4,0,-23,0,4,0,116,0, -68,0,12,0,15,0,-22,0,15,0,-21,0,15,0,-20,0,13,0,-19,0, -13,0,-18,0,7,0,-17,0,4,0,-16,0,4,0,-15,0,4,0,-14,0, -4,0,-13,0,7,0,-53,0,4,0,53,0,69,0,27,0,17,0,-12,0, -15,0,-11,0,15,0,-10,0,13,0,-19,0,13,0,-9,0,13,0,-8,0, -13,0,-7,0,13,0,-6,0,13,0,-5,0,4,0,-4,0,7,0,-3,0, -4,0,-2,0,4,0,-1,0,4,0,0,1,7,0,1,1,7,0,2,1, -4,0,3,1,4,0,4,1,7,0,5,1,7,0,6,1,7,0,7,1, -7,0,8,1,7,0,9,1,7,0,10,1,4,0,11,1,4,0,12,1, -4,0,13,1,70,0,12,0,9,0,14,1,9,0,15,1,13,0,16,1, -7,0,17,1,7,0,18,1,7,0,19,1,4,0,20,1,13,0,21,1, -4,0,22,1,4,0,23,1,4,0,24,1,4,0,53,0,71,0,19,0, -47,0,122,0,68,0,25,1,61,0,26,1,62,0,27,1,63,0,28,1, -64,0,29,1,65,0,30,1,66,0,31,1,69,0,32,1,70,0,33,1, -4,0,34,1,4,0,-1,0,4,0,35,1,4,0,36,1,4,0,37,1, -4,0,38,1,4,0,39,1,4,0,40,1,67,0,41,1,}; +111,77,97,112,68,97,116,97,0,98,116,83,99,97,108,101,100,84,114,105, +97,110,103,108,101,77,101,115,104,83,104,97,112,101,68,97,116,97,0,98, +116,67,111,109,112,111,117,110,100,83,104,97,112,101,67,104,105,108,100,68, +97,116,97,0,98,116,67,111,109,112,111,117,110,100,83,104,97,112,101,68, +97,116,97,0,98,116,67,121,108,105,110,100,101,114,83,104,97,112,101,68, +97,116,97,0,98,116,67,97,112,115,117,108,101,83,104,97,112,101,68,97, +116,97,0,98,116,84,114,105,97,110,103,108,101,73,110,102,111,68,97,116, +97,0,98,116,71,73,109,112,97,99,116,77,101,115,104,83,104,97,112,101, +68,97,116,97,0,98,116,67,111,110,118,101,120,72,117,108,108,83,104,97, +112,101,68,97,116,97,0,98,116,67,111,108,108,105,115,105,111,110,79,98, +106,101,99,116,68,111,117,98,108,101,68,97,116,97,0,98,116,67,111,108, +108,105,115,105,111,110,79,98,106,101,99,116,70,108,111,97,116,68,97,116, +97,0,98,116,82,105,103,105,100,66,111,100,121,70,108,111,97,116,68,97, +116,97,0,98,116,82,105,103,105,100,66,111,100,121,68,111,117,98,108,101, +68,97,116,97,0,98,116,67,111,110,115,116,114,97,105,110,116,73,110,102, +111,49,0,98,116,84,121,112,101,100,67,111,110,115,116,114,97,105,110,116, +68,97,116,97,0,98,116,82,105,103,105,100,66,111,100,121,68,97,116,97, +0,98,116,80,111,105,110,116,50,80,111,105,110,116,67,111,110,115,116,114, +97,105,110,116,70,108,111,97,116,68,97,116,97,0,98,116,80,111,105,110, +116,50,80,111,105,110,116,67,111,110,115,116,114,97,105,110,116,68,111,117, +98,108,101,68,97,116,97,0,98,116,72,105,110,103,101,67,111,110,115,116, +114,97,105,110,116,68,111,117,98,108,101,68,97,116,97,0,98,116,72,105, +110,103,101,67,111,110,115,116,114,97,105,110,116,70,108,111,97,116,68,97, +116,97,0,98,116,67,111,110,101,84,119,105,115,116,67,111,110,115,116,114, +97,105,110,116,68,97,116,97,0,98,116,71,101,110,101,114,105,99,54,68, +111,102,67,111,110,115,116,114,97,105,110,116,68,97,116,97,0,98,116,71, +101,110,101,114,105,99,54,68,111,102,83,112,114,105,110,103,67,111,110,115, +116,114,97,105,110,116,68,97,116,97,0,98,116,83,108,105,100,101,114,67, +111,110,115,116,114,97,105,110,116,68,97,116,97,0,83,111,102,116,66,111, +100,121,77,97,116,101,114,105,97,108,68,97,116,97,0,83,111,102,116,66, +111,100,121,78,111,100,101,68,97,116,97,0,83,111,102,116,66,111,100,121, +76,105,110,107,68,97,116,97,0,83,111,102,116,66,111,100,121,70,97,99, +101,68,97,116,97,0,83,111,102,116,66,111,100,121,84,101,116,114,97,68, +97,116,97,0,83,111,102,116,82,105,103,105,100,65,110,99,104,111,114,68, +97,116,97,0,83,111,102,116,66,111,100,121,67,111,110,102,105,103,68,97, +116,97,0,83,111,102,116,66,111,100,121,80,111,115,101,68,97,116,97,0, +83,111,102,116,66,111,100,121,67,108,117,115,116,101,114,68,97,116,97,0, +98,116,83,111,102,116,66,111,100,121,74,111,105,110,116,68,97,116,97,0, +98,116,83,111,102,116,66,111,100,121,70,108,111,97,116,68,97,116,97,0, +84,76,69,78,1,0,1,0,2,0,2,0,4,0,4,0,4,0,4,0, +8,0,0,0,16,0,48,0,16,0,16,0,32,0,48,0,96,0,64,0, +-128,0,20,0,48,0,80,0,16,0,96,0,-112,0,16,0,56,0,56,0, +20,0,72,0,4,0,4,0,8,0,4,0,56,0,32,0,80,0,72,0, +96,0,80,0,32,0,64,0,64,0,16,0,72,0,80,0,-40,1,8,1, +-16,1,-88,3,8,0,64,0,0,0,96,0,-128,0,104,1,-24,0,-32,0, +8,1,104,1,-40,0,16,0,104,0,24,0,40,0,104,0,96,0,104,0, +-56,0,104,1,112,0,-40,1,83,84,82,67,61,0,0,0,10,0,3,0, +4,0,0,0,4,0,1,0,9,0,2,0,11,0,3,0,10,0,3,0, +10,0,4,0,10,0,5,0,12,0,2,0,9,0,6,0,9,0,7,0, +13,0,1,0,7,0,8,0,14,0,1,0,8,0,8,0,15,0,1,0, +13,0,9,0,16,0,1,0,14,0,9,0,17,0,2,0,15,0,10,0, +13,0,11,0,18,0,2,0,16,0,10,0,14,0,11,0,19,0,4,0, +4,0,12,0,4,0,13,0,2,0,14,0,2,0,15,0,20,0,6,0, +13,0,16,0,13,0,17,0,4,0,18,0,4,0,19,0,4,0,20,0, +0,0,21,0,21,0,6,0,14,0,16,0,14,0,17,0,4,0,18,0, +4,0,19,0,4,0,20,0,0,0,21,0,22,0,3,0,2,0,14,0, +2,0,15,0,4,0,22,0,23,0,12,0,13,0,23,0,13,0,24,0, +13,0,25,0,4,0,26,0,4,0,27,0,4,0,28,0,4,0,29,0, +20,0,30,0,22,0,31,0,19,0,32,0,4,0,33,0,4,0,34,0, +24,0,12,0,14,0,23,0,14,0,24,0,14,0,25,0,4,0,26,0, +4,0,27,0,4,0,28,0,4,0,29,0,21,0,30,0,22,0,31,0, +4,0,33,0,4,0,34,0,19,0,32,0,25,0,3,0,0,0,35,0, +4,0,36,0,0,0,37,0,26,0,5,0,25,0,38,0,13,0,39,0, +13,0,40,0,7,0,41,0,0,0,21,0,27,0,5,0,25,0,38,0, +13,0,39,0,13,0,42,0,7,0,43,0,4,0,44,0,28,0,2,0, +13,0,45,0,7,0,46,0,29,0,4,0,27,0,47,0,28,0,48,0, +4,0,49,0,0,0,37,0,30,0,1,0,4,0,50,0,31,0,2,0, +2,0,50,0,0,0,51,0,32,0,2,0,2,0,52,0,0,0,51,0, +33,0,2,0,0,0,52,0,0,0,53,0,34,0,8,0,13,0,54,0, +14,0,55,0,30,0,56,0,32,0,57,0,33,0,58,0,31,0,59,0, +4,0,60,0,4,0,61,0,35,0,4,0,34,0,62,0,13,0,63,0, +4,0,64,0,0,0,37,0,36,0,7,0,25,0,38,0,35,0,65,0, +23,0,66,0,24,0,67,0,37,0,68,0,7,0,43,0,0,0,69,0, +38,0,2,0,36,0,70,0,13,0,39,0,39,0,4,0,17,0,71,0, +25,0,72,0,4,0,73,0,7,0,74,0,40,0,4,0,25,0,38,0, +39,0,75,0,4,0,76,0,7,0,43,0,41,0,3,0,27,0,47,0, +4,0,77,0,0,0,37,0,42,0,3,0,27,0,47,0,4,0,77,0, +0,0,37,0,43,0,4,0,4,0,78,0,7,0,79,0,7,0,80,0, +7,0,81,0,37,0,14,0,4,0,82,0,4,0,83,0,43,0,84,0, +4,0,85,0,7,0,86,0,7,0,87,0,7,0,88,0,7,0,89,0, +7,0,90,0,4,0,91,0,4,0,92,0,4,0,93,0,4,0,94,0, +0,0,37,0,44,0,5,0,25,0,38,0,35,0,65,0,13,0,39,0, +7,0,43,0,4,0,95,0,45,0,5,0,27,0,47,0,13,0,96,0, +14,0,97,0,4,0,98,0,0,0,99,0,46,0,24,0,9,0,100,0, +9,0,101,0,25,0,102,0,0,0,35,0,18,0,103,0,18,0,104,0, +14,0,105,0,14,0,106,0,14,0,107,0,8,0,108,0,8,0,109,0, +8,0,110,0,8,0,111,0,8,0,112,0,8,0,113,0,8,0,114,0, +4,0,115,0,4,0,116,0,4,0,117,0,4,0,118,0,4,0,119,0, +4,0,120,0,4,0,121,0,0,0,37,0,47,0,23,0,9,0,100,0, +9,0,101,0,25,0,102,0,0,0,35,0,17,0,103,0,17,0,104,0, +13,0,105,0,13,0,106,0,13,0,107,0,7,0,108,0,7,0,109,0, +7,0,110,0,7,0,111,0,7,0,112,0,7,0,113,0,7,0,114,0, +4,0,115,0,4,0,116,0,4,0,117,0,4,0,118,0,4,0,119,0, +4,0,120,0,4,0,121,0,48,0,21,0,47,0,122,0,15,0,123,0, +13,0,124,0,13,0,125,0,13,0,126,0,13,0,127,0,13,0,-128,0, +13,0,-127,0,13,0,-126,0,13,0,-125,0,13,0,-124,0,7,0,-123,0, +7,0,-122,0,7,0,-121,0,7,0,-120,0,7,0,-119,0,7,0,-118,0, +7,0,-117,0,7,0,-116,0,7,0,-115,0,4,0,-114,0,49,0,22,0, +46,0,122,0,16,0,123,0,14,0,124,0,14,0,125,0,14,0,126,0, +14,0,127,0,14,0,-128,0,14,0,-127,0,14,0,-126,0,14,0,-125,0, +14,0,-124,0,8,0,-123,0,8,0,-122,0,8,0,-121,0,8,0,-120,0, +8,0,-119,0,8,0,-118,0,8,0,-117,0,8,0,-116,0,8,0,-115,0, +4,0,-114,0,0,0,37,0,50,0,2,0,4,0,-113,0,4,0,-112,0, +51,0,13,0,52,0,-111,0,52,0,-110,0,0,0,35,0,4,0,-109,0, +4,0,-108,0,4,0,-107,0,4,0,-106,0,7,0,-105,0,7,0,-104,0, +4,0,-103,0,4,0,-102,0,7,0,-101,0,4,0,-100,0,53,0,3,0, +51,0,-99,0,13,0,-98,0,13,0,-97,0,54,0,3,0,51,0,-99,0, +14,0,-98,0,14,0,-97,0,55,0,13,0,51,0,-99,0,18,0,-96,0, +18,0,-95,0,4,0,-94,0,4,0,-93,0,4,0,-92,0,7,0,-91,0, +7,0,-90,0,7,0,-89,0,7,0,-88,0,7,0,-87,0,7,0,-86,0, +7,0,-85,0,56,0,13,0,51,0,-99,0,17,0,-96,0,17,0,-95,0, +4,0,-94,0,4,0,-93,0,4,0,-92,0,7,0,-91,0,7,0,-90,0, +7,0,-89,0,7,0,-88,0,7,0,-87,0,7,0,-86,0,7,0,-85,0, +57,0,11,0,51,0,-99,0,17,0,-96,0,17,0,-95,0,7,0,-84,0, +7,0,-83,0,7,0,-82,0,7,0,-87,0,7,0,-86,0,7,0,-85,0, +7,0,-81,0,0,0,21,0,58,0,9,0,51,0,-99,0,17,0,-96,0, +17,0,-95,0,13,0,-80,0,13,0,-79,0,13,0,-78,0,13,0,-77,0, +4,0,-76,0,4,0,-75,0,59,0,5,0,58,0,-74,0,4,0,-73,0, +7,0,-72,0,7,0,-71,0,7,0,-70,0,60,0,9,0,51,0,-99,0, +17,0,-96,0,17,0,-95,0,7,0,-80,0,7,0,-79,0,7,0,-78,0, +7,0,-77,0,4,0,-76,0,4,0,-75,0,61,0,4,0,7,0,-69,0, +7,0,-68,0,7,0,-67,0,4,0,78,0,62,0,10,0,61,0,-66,0, +13,0,-65,0,13,0,-64,0,13,0,-63,0,13,0,-62,0,13,0,-61,0, +7,0,-123,0,7,0,-60,0,4,0,-59,0,4,0,53,0,63,0,4,0, +61,0,-66,0,4,0,-58,0,7,0,-57,0,4,0,-56,0,64,0,4,0, +13,0,-61,0,61,0,-66,0,4,0,-55,0,7,0,-54,0,65,0,7,0, +13,0,-53,0,61,0,-66,0,4,0,-52,0,7,0,-51,0,7,0,-50,0, +7,0,-49,0,4,0,53,0,66,0,6,0,15,0,-48,0,13,0,-50,0, +13,0,-47,0,52,0,-46,0,4,0,-45,0,7,0,-49,0,67,0,26,0, +4,0,-44,0,7,0,-43,0,7,0,-81,0,7,0,-42,0,7,0,-41,0, +7,0,-40,0,7,0,-39,0,7,0,-38,0,7,0,-37,0,7,0,-36,0, +7,0,-35,0,7,0,-34,0,7,0,-33,0,7,0,-32,0,7,0,-31,0, +7,0,-30,0,7,0,-29,0,7,0,-28,0,7,0,-27,0,7,0,-26,0, +7,0,-25,0,4,0,-24,0,4,0,-23,0,4,0,-22,0,4,0,-21,0, +4,0,116,0,68,0,12,0,15,0,-20,0,15,0,-19,0,15,0,-18,0, +13,0,-17,0,13,0,-16,0,7,0,-15,0,4,0,-14,0,4,0,-13,0, +4,0,-12,0,4,0,-11,0,7,0,-51,0,4,0,53,0,69,0,27,0, +17,0,-10,0,15,0,-9,0,15,0,-8,0,13,0,-17,0,13,0,-7,0, +13,0,-6,0,13,0,-5,0,13,0,-4,0,13,0,-3,0,4,0,-2,0, +7,0,-1,0,4,0,0,1,4,0,1,1,4,0,2,1,7,0,3,1, +7,0,4,1,4,0,5,1,4,0,6,1,7,0,7,1,7,0,8,1, +7,0,9,1,7,0,10,1,7,0,11,1,7,0,12,1,4,0,13,1, +4,0,14,1,4,0,15,1,70,0,12,0,9,0,16,1,9,0,17,1, +13,0,18,1,7,0,19,1,7,0,20,1,7,0,21,1,4,0,22,1, +13,0,23,1,4,0,24,1,4,0,25,1,4,0,26,1,4,0,53,0, +71,0,19,0,47,0,122,0,68,0,27,1,61,0,28,1,62,0,29,1, +63,0,30,1,64,0,31,1,65,0,32,1,66,0,33,1,69,0,34,1, +70,0,35,1,4,0,36,1,4,0,1,1,4,0,37,1,4,0,38,1, +4,0,39,1,4,0,40,1,4,0,41,1,4,0,42,1,67,0,43,1, +}; int sBulletDNAlen64= sizeof(sBulletDNAstr64); diff --git a/extern/bullet2/src/LinearMath/btSerializer.h b/extern/bullet2/src/LinearMath/btSerializer.h index 52520d72b9e..76f3cf32f8e 100644 --- a/extern/bullet2/src/LinearMath/btSerializer.h +++ b/extern/bullet2/src/LinearMath/btSerializer.h @@ -28,9 +28,9 @@ subject to the following restrictions: ///only the 32bit versions for now -extern unsigned char sBulletDNAstr[]; +extern char sBulletDNAstr[]; extern int sBulletDNAlen; -extern unsigned char sBulletDNAstr64[]; +extern char sBulletDNAstr64[]; extern int sBulletDNAlen64; SIMD_FORCE_INLINE int btStrLen(const char* str) @@ -213,7 +213,7 @@ protected: int *intPtr=0; short *shtPtr=0; - char *cp = 0;int dataLen =0;long nr=0; + char *cp = 0;int dataLen =0; intPtr = (int*)m_dna; /* @@ -246,15 +246,7 @@ protected: while (*cp)cp++; cp++; } - { - nr= (long)cp; - // long mask=3; - nr= ((nr+3)&~3)-nr; - while (nr--) - { - cp++; - } - } + cp = btAlignPointer(cp,4); /* TYPE (4 bytes) @@ -281,15 +273,7 @@ protected: cp++; } - { - nr= (long)cp; - // long mask=3; - nr= ((nr+3)&~3)-nr; - while (nr--) - { - cp++; - } - } + cp = btAlignPointer(cp,4); /* @@ -453,8 +437,8 @@ public: buffer[9] = '2'; - buffer[10] = '7'; - buffer[11] = '8'; + buffer[10] = '8'; + buffer[11] = '0'; } diff --git a/extern/bullet2/src/LinearMath/btSimdMinMax.h b/extern/bullet2/src/LinearMath/btSimdMinMax.h deleted file mode 100644 index 75e83f3c53f..00000000000 --- a/extern/bullet2/src/LinearMath/btSimdMinMax.h +++ /dev/null @@ -1,41 +0,0 @@ -/* -Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - - -#ifndef SIMD_MINMAX_H -#define SIMD_MINMAX_H -#include "btScalar.h" - -template <class T> -SIMD_FORCE_INLINE const T& btMin(const T& a, const T& b) { - return b < a ? b : a; -} - -template <class T> -SIMD_FORCE_INLINE const T& btMax(const T& a, const T& b) { - return a < b ? b : a; -} - -template <class T> -SIMD_FORCE_INLINE void btSetMin(T& a, const T& b) { - if (a > b) a = b; -} - -template <class T> -SIMD_FORCE_INLINE void btSetMax(T& a, const T& b) { - if (a < b) a = b; -} - -#endif diff --git a/extern/bullet2/src/LinearMath/btTransform.h b/extern/bullet2/src/LinearMath/btTransform.h index 187b09116cc..5e52d183acb 100644 --- a/extern/bullet2/src/LinearMath/btTransform.h +++ b/extern/bullet2/src/LinearMath/btTransform.h @@ -14,8 +14,8 @@ subject to the following restrictions: -#ifndef btTransform_H -#define btTransform_H +#ifndef BT_TRANSFORM_H +#define BT_TRANSFORM_H #include "btMatrix3x3.h" @@ -298,7 +298,7 @@ SIMD_FORCE_INLINE void btTransform::deSerializeDouble(const btTransformDoubleDat } -#endif +#endif //BT_TRANSFORM_H diff --git a/extern/bullet2/src/LinearMath/btTransformUtil.h b/extern/bullet2/src/LinearMath/btTransformUtil.h index 626110ce4ee..2303c274275 100644 --- a/extern/bullet2/src/LinearMath/btTransformUtil.h +++ b/extern/bullet2/src/LinearMath/btTransformUtil.h @@ -13,8 +13,8 @@ subject to the following restrictions: */ -#ifndef SIMD_TRANSFORM_UTIL_H -#define SIMD_TRANSFORM_UTIL_H +#ifndef BT_TRANSFORM_UTIL_H +#define BT_TRANSFORM_UTIL_H #include "btTransform.h" #define ANGULAR_MOTION_THRESHOLD btScalar(0.5)*SIMD_HALF_PI @@ -224,5 +224,5 @@ public: }; -#endif //SIMD_TRANSFORM_UTIL_H +#endif //BT_TRANSFORM_UTIL_H diff --git a/extern/bullet2/src/LinearMath/btVector3.h b/extern/bullet2/src/LinearMath/btVector3.h index 068e87c94ab..d99b7c83ae3 100644 --- a/extern/bullet2/src/LinearMath/btVector3.h +++ b/extern/bullet2/src/LinearMath/btVector3.h @@ -14,8 +14,8 @@ subject to the following restrictions: -#ifndef SIMD__VECTOR3_H -#define SIMD__VECTOR3_H +#ifndef BT_VECTOR3_H +#define BT_VECTOR3_H #include "btScalar.h" @@ -763,4 +763,4 @@ SIMD_FORCE_INLINE void btVector3::deSerialize(const struct btVector3Data& dataIn } -#endif //SIMD__VECTOR3_H +#endif //BT_VECTOR3_H diff --git a/extern/bullet2/src/SConscript b/extern/bullet2/src/SConscript index cd94df257ec..8a3db17f6d1 100644 --- a/extern/bullet2/src/SConscript +++ b/extern/bullet2/src/SConscript @@ -21,25 +21,18 @@ elif sys.platform=='darwin': defs += ' NDEBUG' cflags += ['-O2','-pipe', '-fPIC', '-funsigned-char', '-ffast-math'] -linearmath_src = env.Glob("LinearMath/*.cpp") - -bulletdyn_src = env.Glob("BulletDynamics/Vehicle/*.cpp") + env.Glob("BulletDynamics/ConstraintSolver/*.cpp") + env.Glob("BulletDynamics/Dynamics/*.cpp") + env.Glob("BulletDynamics/Character/*.cpp") - -collision_broadphase_src = env.Glob("BulletCollision/BroadphaseCollision/*.cpp") -collision_dispatch_src = env.Glob("BulletCollision/CollisionDispatch/*.cpp") -collision_gimpact_src = env.Glob("BulletCollision/Gimpact/*.cpp") -collision_shapes_src = env.Glob("BulletCollision/CollisionShapes/*.cpp") -collision_narrowphase_src = env.Glob("BulletCollision/NarrowPhaseCollision/*.cpp") - -softbody_src = env.Glob("BulletSoftBody/*.cpp") +bullet2_src = env.Glob("LinearMath/*.cpp") +bullet2_src += env.Glob("BulletCollision/BroadphaseCollision/*.cpp") +bullet2_src += env.Glob("BulletCollision/CollisionDispatch/*.cpp") +bullet2_src += env.Glob("BulletCollision/Gimpact/*.cpp") +bullet2_src += env.Glob("BulletCollision/CollisionShapes/*.cpp") +bullet2_src += env.Glob("BulletCollision/NarrowPhaseCollision/*.cpp") +bullet2_src += env.Glob("BulletDynamics/Vehicle/*.cpp") +bullet2_src += env.Glob("BulletDynamics/ConstraintSolver/*.cpp") +bullet2_src += env.Glob("BulletDynamics/Dynamics/*.cpp") +bullet2_src += env.Glob("BulletDynamics/Character/*.cpp") +bullet2_src += env.Glob("BulletSoftBody/*.cpp") incs = '. BulletCollision BulletDynamics LinearMath BulletSoftBody' -env.BlenderLib ( libname = 'extern_bullet2linmath', sources=linearmath_src, includes=Split(incs), defines=Split(defs), libtype=['extern','player'], priority=[20,137], compileflags=cflags ) -env.BlenderLib ( libname = 'extern_bullet2dynamics', sources=bulletdyn_src, includes=Split(incs), defines=Split(defs), libtype=['extern','player'], priority=[19,136], compileflags=cflags ) -env.BlenderLib ( libname = 'extern_bullet2collision_broadphase', sources=collision_broadphase_src, includes=Split(incs), defines=Split(defs), libtype=['extern','player'], priority=[25,145], compileflags=cflags ) -env.BlenderLib ( libname = 'extern_bullet2collision_dispatch', sources=collision_dispatch_src, includes=Split(incs), defines=Split(defs), libtype=['extern','player'], priority=[20,138], compileflags=cflags ) -env.BlenderLib ( libname = 'extern_bullet2collision_gimpact', sources=collision_gimpact_src, includes=Split(incs), defines=Split(defs), libtype=['extern','player'], priority=[20,138], compileflags=cflags ) -env.BlenderLib ( libname = 'extern_bullet2collision_shapes', sources=collision_shapes_src, includes=Split(incs), defines=Split(defs), libtype=['extern','player'], priority=[20,138], compileflags=cflags ) -env.BlenderLib ( libname = 'extern_bullet2collision_narrowphase', sources=collision_narrowphase_src, includes=Split(incs), defines=Split(defs), libtype=['extern','player'], priority=[20,138], compileflags=cflags ) -env.BlenderLib ( libname = 'extern_bullet2softbody', sources=softbody_src, includes=Split(incs), defines=Split(defs), libtype=['extern','player'], priority=[30,135], compileflags=cflags ) +env.BlenderLib ( libname = 'extern_bullet2', sources=bullet2_src, includes=Split(incs), defines=Split(defs), libtype=['extern','player'], priority=[20,137], compileflags=cflags ) diff --git a/extern/bullet2/src/btBulletDynamicsCommon.h b/extern/bullet2/src/btBulletDynamicsCommon.h index db8b37989f5..ccfad19b87a 100644 --- a/extern/bullet2/src/btBulletDynamicsCommon.h +++ b/extern/bullet2/src/btBulletDynamicsCommon.h @@ -20,7 +20,6 @@ subject to the following restrictions: #include "btBulletCollisionCommon.h" #include "BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h" -#include "BulletDynamics/Dynamics/btContinuousDynamicsWorld.h" #include "BulletDynamics/Dynamics/btSimpleDynamicsWorld.h" #include "BulletDynamics/Dynamics/btRigidBody.h" diff --git a/extern/libmv/CMakeLists.txt b/extern/libmv/CMakeLists.txt index 6be813883ec..60cd84d89d4 100644 --- a/extern/libmv/CMakeLists.txt +++ b/extern/libmv/CMakeLists.txt @@ -32,6 +32,7 @@ set(INC third_party/ssba third_party/ldl/Include ../colamd/Include + third_party/ceres/include ) set(INC_SYS @@ -61,6 +62,7 @@ set(SRC libmv/multiview/fundamental.cc libmv/multiview/projection.cc libmv/multiview/triangulation.cc + libmv/multiview/homography.cc libmv/numeric/numeric.cc libmv/numeric/poly.cc libmv/simple_pipeline/bundle.cc @@ -83,6 +85,7 @@ set(SRC libmv/tracking/pyramid_region_tracker.cc libmv/tracking/retrack_region_tracker.cc libmv/tracking/trklt_region_tracker.cc + libmv/tracking/track_region.cc third_party/fast/fast_10.c third_party/fast/fast_11.c @@ -250,3 +253,5 @@ add_definitions( ) blender_add_lib(extern_libmv "${SRC}" "${INC}" "${INC_SYS}") + +add_subdirectory(third_party) diff --git a/extern/libmv/SConscript b/extern/libmv/SConscript index fbb6ee36f85..b47086f3e91 100644 --- a/extern/libmv/SConscript +++ b/extern/libmv/SConscript @@ -30,7 +30,7 @@ src += env.Glob('third_party/ldl/Source/*.c') src += env.Glob('third_party/ssba/Geometry/*.cpp') src += env.Glob('third_party/ssba/Math/*.cpp') -incs = '. ../Eigen3' +incs = '. ../Eigen3 third_party/ceres/include' incs += ' ' + env['BF_PNG_INC'] incs += ' ' + env['BF_ZLIB_INC'] @@ -65,3 +65,5 @@ else: incs += ' ./third_party/ssba ./third_party/ldl/Include ../colamd/Include' env.BlenderLib ( libname = 'extern_libmv', sources=src, includes=Split(incs), defines=defs, libtype=['extern', 'player'], priority=[20,137], compileflags=cflags_libmv, cc_compileflags=ccflags_libmv, cxx_compileflags=cxxflags_libmv ) + +SConscript(['third_party/SConscript']) diff --git a/extern/libmv/bundle.sh b/extern/libmv/bundle.sh index 3f58097b881..3f877508c46 100755 --- a/extern/libmv/bundle.sh +++ b/extern/libmv/bundle.sh @@ -25,7 +25,9 @@ for p in `cat ./patches/series`; do done find libmv -type f -not -iwholename '*.svn*' -exec rm -rf {} \; -find third_party -type f -not -iwholename '*.svn*' -exec rm -rf {} \; +find third_party -type f -not -iwholename '*.svn*' -not -iwholename '*third_party/ceres*' \ + -not -iwholename '*third_party/SConscript*' -not -iwholename '*third_party/CMakeLists.txt*' \ + -exec rm -rf {} \; cat "files.txt" | while read f; do mkdir -p `dirname $f` @@ -39,14 +41,14 @@ chmod 664 ./third_party/glog/src/windows/*.cc ./third_party/glog/src/windows/*.h sources=`find ./libmv -type f -iname '*.cc' -or -iname '*.cpp' -or -iname '*.c' | sed -r 's/^\.\//\t/' | sort -d` headers=`find ./libmv -type f -iname '*.h' | sed -r 's/^\.\//\t/' | sort -d` -third_sources=`find ./third_party -type f -iname '*.cc' -or -iname '*.cpp' -or -iname '*.c' | grep -v glog | sed -r 's/^\.\//\t/' | sort -d` -third_headers=`find ./third_party -type f -iname '*.h' | grep -v glog | sed -r 's/^\.\//\t/' | sort -d` +third_sources=`find ./third_party -type f -iname '*.cc' -or -iname '*.cpp' -or -iname '*.c' | grep -v glog | grep -v ceres | sed -r 's/^\.\//\t/' | sort -d` +third_headers=`find ./third_party -type f -iname '*.h' | grep -v glog | grep -v ceres | sed -r 's/^\.\//\t/' | sort -d` third_glog_sources=`find ./third_party -type f -iname '*.cc' -or -iname '*.cpp' -or -iname '*.c' | grep glog | grep -v windows | sed -r 's/^\.\//\t\t/' | sort -d` third_glog_headers=`find ./third_party -type f -iname '*.h' | grep glog | grep -v windows | sed -r 's/^\.\//\t\t/' | sort -d` src_dir=`find ./libmv -type f -iname '*.cc' -exec dirname {} \; -or -iname '*.cpp' -exec dirname {} \; -or -iname '*.c' -exec dirname {} \; | sed -r 's/^\.\//\t/' | sort -d | uniq` -src_third_dir=`find ./third_party -type f -iname '*.cc' -exec dirname {} \; -or -iname '*.cpp' -exec dirname {} \; -or -iname '*.c' -exec dirname {} \; | sed -r 's/^\.\//\t/' | sort -d | uniq` +src_third_dir=`find ./third_party -type f -iname '*.cc' -exec dirname {} \; -or -iname '*.cpp' -exec dirname {} \; -or -iname '*.c' -exec dirname {} \; | grep -v ceres | sed -r 's/^\.\//\t/' | sort -d | uniq` src="" win_src="" for x in $src_dir $src_third_dir; do @@ -126,6 +128,7 @@ set(INC third_party/ssba third_party/ldl/Include ../colamd/Include + third_party/ceres/include ) set(INC_SYS @@ -220,6 +223,8 @@ add_definitions( ) blender_add_lib(extern_libmv "\${SRC}" "\${INC}" "\${INC_SYS}") + +add_subdirectory(third_party) EOF cat > SConscript << EOF @@ -246,7 +251,7 @@ defs.append('GOOGLE_GLOG_DLL_DECL=') src = env.Glob("*.cpp") $src -incs = '. ../Eigen3' +incs = '. ../Eigen3 third_party/ceres/include' incs += ' ' + env['BF_PNG_INC'] incs += ' ' + env['BF_ZLIB_INC'] @@ -281,4 +286,6 @@ else: incs += ' ./third_party/ssba ./third_party/ldl/Include ../colamd/Include' env.BlenderLib ( libname = 'extern_libmv', sources=src, includes=Split(incs), defines=defs, libtype=['extern', 'player'], priority=[20,137], compileflags=cflags_libmv, cc_compileflags=ccflags_libmv, cxx_compileflags=cxxflags_libmv ) + +SConscript(['third_party/SConscript']) EOF diff --git a/extern/libmv/libmv-capi.cpp b/extern/libmv/libmv-capi.cpp index 6c20d76eeac..ffa065f08fe 100644 --- a/extern/libmv/libmv-capi.cpp +++ b/extern/libmv/libmv-capi.cpp @@ -28,6 +28,10 @@ tracking between which failed */ #undef DUMP_FAILURE +/* define this to generate PNG images with content of search areas + on every itteration of tracking */ +#undef DUMP_ALWAYS + #include "libmv-capi.h" #include "third_party/gflags/gflags/gflags.h" @@ -45,6 +49,7 @@ #include "libmv/tracking/trklt_region_tracker.h" #include "libmv/tracking/lmicklt_region_tracker.h" #include "libmv/tracking/pyramid_region_tracker.h" +#include "libmv/tracking/track_region.h" #include "libmv/simple_pipeline/callbacks.h" #include "libmv/simple_pipeline/tracks.h" @@ -59,7 +64,7 @@ #include <stdlib.h> #include <assert.h> -#ifdef DUMP_FAILURE +#if defined(DUMP_FAILURE) || defined (DUMP_ALWAYS) # include <png.h> #endif @@ -97,7 +102,7 @@ void libmv_initLogging(const char *argv0) void libmv_startDebugLogging(void) { google::SetCommandLineOption("logtostderr", "1"); - google::SetCommandLineOption("v", "0"); + google::SetCommandLineOption("v", "2"); google::SetCommandLineOption("stderrthreshold", "1"); google::SetCommandLineOption("minloglevel", "0"); V3D::optimizerVerbosenessLevel = 1; @@ -158,20 +163,35 @@ libmv_RegionTracker *libmv_bruteRegionTrackerNew(int half_window_size, double mi return (libmv_RegionTracker *)brute_region_tracker; } -static void floatBufToImage(const float *buf, int width, int height, libmv::FloatImage *image) +static void floatBufToImage(const float *buf, int width, int height, int channels, libmv::FloatImage *image) { - int x, y, a = 0; + int x, y, k, a = 0; - image->resize(height, width); + image->Resize(height, width, channels); for (y = 0; y < height; y++) { for (x = 0; x < width; x++) { - (*image)(y, x, 0) = buf[a++]; + for (k = 0; k < channels; k++) { + (*image)(y, x, k) = buf[a++]; + } + } + } +} + +static void imageToFloatBuf(const libmv::FloatImage *image, int channels, float *buf) +{ + int x, y, k, a = 0; + + for (y = 0; y < image->Height(); y++) { + for (x = 0; x < image->Width(); x++) { + for (k = 0; k < channels; k++) { + buf[a++] = (*image)(y, x, k); + } } } } -#ifdef DUMP_FAILURE +#if defined(DUMP_FAILURE) || defined (DUMP_ALWAYS) void savePNGImage(png_bytep *row_pointers, int width, int height, int depth, int color_type, char *file_name) { png_infop info_ptr; @@ -234,14 +254,14 @@ static void saveImage(char *prefix, libmv::FloatImage image, int x0, int y0) row_pointers[y]= (png_bytep)malloc(sizeof(png_byte)*4*image.Width()); for (x = 0; x < image.Width(); x++) { - if (x0 == x && y0 == y) { + if (x0 == x && image.Height() - y0 - 1 == y) { row_pointers[y][x*4+0]= 255; row_pointers[y][x*4+1]= 0; row_pointers[y][x*4+2]= 0; row_pointers[y][x*4+3]= 255; } else { - float pixel = image(y, x, 0); + float pixel = image(image.Height() - y - 1, x, 0); row_pointers[y][x*4+0]= pixel*255; row_pointers[y][x*4+1]= pixel*255; row_pointers[y][x*4+2]= pixel*255; @@ -302,19 +322,23 @@ int libmv_regionTrackerTrack(libmv_RegionTracker *libmv_tracker, const float *im libmv::RegionTracker *region_tracker = (libmv::RegionTracker *)libmv_tracker; libmv::FloatImage old_patch, new_patch; - floatBufToImage(ima1, width, height, &old_patch); - floatBufToImage(ima2, width, height, &new_patch); + floatBufToImage(ima1, width, height, 1, &old_patch); + floatBufToImage(ima2, width, height, 1, &new_patch); -#ifndef DUMP_FAILURE +#if !defined(DUMP_FAILURE) && !defined(DUMP_ALWAYS) return region_tracker->Track(old_patch, new_patch, x1, y1, x2, y2); #else { - double sx2 = *x2, sy2 = *y2; + /* double sx2 = *x2, sy2 = *y2; */ int result = region_tracker->Track(old_patch, new_patch, x1, y1, x2, y2); +#if defined(DUMP_ALWAYS) + { +#else if (!result) { +#endif saveImage("old_patch", old_patch, x1, y1); - saveImage("new_patch", new_patch, sx2, sy2); + saveImage("new_patch", new_patch, *x2, *y2); } return result; @@ -329,6 +353,103 @@ void libmv_regionTrackerDestroy(libmv_RegionTracker *libmv_tracker) delete region_tracker; } +/* ************ Planar tracker ************ */ + +/* TrackRegion (new planar tracker) */ +int libmv_trackRegion(const struct libmv_trackRegionOptions *options, + const float *image1, const float *image2, + int width, int height, + const double *x1, const double *y1, + struct libmv_trackRegionResult *result, + double *x2, double *y2) +{ + double xx1[5], yy1[5]; + double xx2[5], yy2[5]; + bool tracking_result = false; + + /* Convert to doubles for the libmv api. The four corners and the center. */ + for (int i = 0; i < 5; ++i) { + xx1[i] = x1[i]; + yy1[i] = y1[i]; + xx2[i] = x2[i]; + yy2[i] = y2[i]; + } + + libmv::TrackRegionOptions track_region_options; + switch (options->motion_model) { +#define LIBMV_CONVERT(the_model) \ + case libmv::TrackRegionOptions::the_model: \ + track_region_options.mode = libmv::TrackRegionOptions::the_model; \ + break; + LIBMV_CONVERT(TRANSLATION) + LIBMV_CONVERT(TRANSLATION_ROTATION) + LIBMV_CONVERT(TRANSLATION_SCALE) + LIBMV_CONVERT(TRANSLATION_ROTATION_SCALE) + LIBMV_CONVERT(AFFINE) + LIBMV_CONVERT(HOMOGRAPHY) +#undef LIBMV_CONVERT + } + + track_region_options.minimum_correlation = options->minimum_correlation; + track_region_options.max_iterations = options->num_iterations; + track_region_options.sigma = options->sigma; + track_region_options.num_extra_points = 1; + track_region_options.image1_mask = NULL; + track_region_options.use_brute_initialization = options->use_brute; + track_region_options.use_normalized_intensities = options->use_normalization; + + /* Convert from raw float buffers to libmv's FloatImage. */ + libmv::FloatImage old_patch, new_patch; + floatBufToImage(image1, width, height, 1, &old_patch); + floatBufToImage(image2, width, height, 1, &new_patch); + + libmv::TrackRegionResult track_region_result; + libmv::TrackRegion(old_patch, new_patch, xx1, yy1, track_region_options, xx2, yy2, &track_region_result); + + /* Convert to floats for the blender api. */ + for (int i = 0; i < 5; ++i) { + x2[i] = xx2[i]; + y2[i] = yy2[i]; + } + + /* TODO(keir): Update the termination string with failure details. */ + if (track_region_result.termination == libmv::TrackRegionResult::PARAMETER_TOLERANCE || + track_region_result.termination == libmv::TrackRegionResult::FUNCTION_TOLERANCE || + track_region_result.termination == libmv::TrackRegionResult::GRADIENT_TOLERANCE || + track_region_result.termination == libmv::TrackRegionResult::NO_CONVERGENCE) + { + tracking_result = true; + } + +#if defined(DUMP_FAILURE) || defined(DUMP_ALWAYS) +#if defined(DUMP_ALWAYS) + { +#else + if (!tracking_result) { +#endif + saveImage("old_patch", old_patch, x1[4], y1[4]); + saveImage("new_patch", new_patch, x2[4], y2[4]); + } +#endif + + return tracking_result; +} + +void libmv_samplePlanarPatch(const float *image, int width, int height, + int channels, const double *xs, const double *ys, + int num_samples_x, int num_samples_y, float *patch, + double *warped_position_x, double *warped_position_y) +{ + libmv::FloatImage libmv_image, libmv_patch; + + floatBufToImage(image, width, height, channels, &libmv_image); + + libmv::SamplePlanarPatch(libmv_image, xs, ys, num_samples_x, num_samples_y, + &libmv_patch, warped_position_x, warped_position_y); + + imageToFloatBuf(&libmv_patch, channels, patch); +} + /* ************ Tracks ************ */ libmv_Tracks *libmv_tracksNew(void) diff --git a/extern/libmv/libmv-capi.h b/extern/libmv/libmv-capi.h index bccc4706832..6f4b5dea384 100644 --- a/extern/libmv/libmv-capi.h +++ b/extern/libmv/libmv-capi.h @@ -50,6 +50,32 @@ int libmv_regionTrackerTrack(struct libmv_RegionTracker *libmv_tracker, const fl int width, int height, double x1, double y1, double *x2, double *y2); void libmv_regionTrackerDestroy(struct libmv_RegionTracker *libmv_tracker); +/* TrackRegion (new planar tracker) */ +struct libmv_trackRegionOptions { + int motion_model; + int num_iterations; + int use_brute; + int use_normalization; + double minimum_correlation; + double sigma; +}; +struct libmv_trackRegionResult { + int termination; + const char *termination_reason; + double correlation; +}; +int libmv_trackRegion(const struct libmv_trackRegionOptions *options, + const float *image1, const float *image2, + int width, int height, + const double *x1, const double *y1, + struct libmv_trackRegionResult *result, + double *x2, double *y2); + +void libmv_samplePlanarPatch(const float *image, int width, int height, + int channels, const double *xs, const double *ys, + int num_samples_x, int num_samples_y, float *patch, + double *warped_position_x, double *warped_position_y); + /* Tracks */ struct libmv_Tracks *libmv_tracksNew(void); void libmv_tracksInsert(struct libmv_Tracks *libmv_tracks, int image, int track, double x, double y); diff --git a/extern/libmv/libmv/image/sample.h b/extern/libmv/libmv/image/sample.h index e842747e6d4..a8850effeab 100644 --- a/extern/libmv/libmv/image/sample.h +++ b/extern/libmv/libmv/image/sample.h @@ -34,25 +34,22 @@ inline T SampleNearest(const Array3D<T> &image, return image(i, j, v); } -static inline void LinearInitAxis(float fx, int width, - int *x1, int *x2, - float *dx1, float *dx2) { - const int ix = int(fx); +inline void LinearInitAxis(float x, int size, + int *x1, int *x2, + float *dx) { + const int ix = static_cast<int>(x); if (ix < 0) { *x1 = 0; *x2 = 0; - *dx1 = 1; - *dx2 = 0; - } else if (ix > width-2) { - *x1 = width-1; - *x2 = width-1; - *dx1 = 1; - *dx2 = 0; + *dx = 1.0; + } else if (ix > size - 2) { + *x1 = size - 1; + *x2 = size - 1; + *dx = 1.0; } else { *x1 = ix; - *x2 = *x1 + 1; - *dx1 = *x2 - fx; - *dx2 = 1 - *dx1; + *x2 = ix + 1; + *dx = *x2 - x; } } @@ -60,18 +57,47 @@ static inline void LinearInitAxis(float fx, int width, template<typename T> inline T SampleLinear(const Array3D<T> &image, float y, float x, int v = 0) { int x1, y1, x2, y2; - float dx1, dy1, dx2, dy2; + float dx, dy; - LinearInitAxis(y, image.Height(), &y1, &y2, &dy1, &dy2); - LinearInitAxis(x, image.Width(), &x1, &x2, &dx1, &dx2); + // Take the upper left corner as integer pixel positions. + x -= 0.5; + y -= 0.5; + + LinearInitAxis(y, image.Height(), &y1, &y2, &dy); + LinearInitAxis(x, image.Width(), &x1, &x2, &dx); const T im11 = image(y1, x1, v); const T im12 = image(y1, x2, v); const T im21 = image(y2, x1, v); const T im22 = image(y2, x2, v); - return T(dy1 * ( dx1 * im11 + dx2 * im12 ) + - dy2 * ( dx1 * im21 + dx2 * im22 )); + return T( dy * ( dx * im11 + (1.0 - dx) * im12 ) + + (1 - dy) * ( dx * im21 + (1.0 - dx) * im22 )); +} + +/// Linear interpolation, of all channels. The sample is assumed to have the +/// same size as the number of channels in image. +template<typename T> +inline void SampleLinear(const Array3D<T> &image, float y, float x, T *sample) { + int x1, y1, x2, y2; + float dx, dy; + + // Take the upper left corner as integer pixel positions. + x -= 0.5; + y -= 0.5; + + LinearInitAxis(y, image.Height(), &y1, &y2, &dy); + LinearInitAxis(x, image.Width(), &x1, &x2, &dx); + + for (int i = 0; i < image.Depth(); ++i) { + const T im11 = image(y1, x1, i); + const T im12 = image(y1, x2, i); + const T im21 = image(y2, x1, i); + const T im22 = image(y2, x2, i); + + sample[i] = T( dy * ( dx * im11 + (1.0 - dx) * im12 ) + + (1 - dy) * ( dx * im21 + (1.0 - dx) * im22 )); + } } // Downsample all channels by 2. If the image has odd width or height, the last diff --git a/extern/libmv/libmv/multiview/homography.cc b/extern/libmv/libmv/multiview/homography.cc new file mode 100644 index 00000000000..366392f3923 --- /dev/null +++ b/extern/libmv/libmv/multiview/homography.cc @@ -0,0 +1,267 @@ +// Copyright (c) 2008, 2009 libmv authors. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to +// deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. + +#include "libmv/logging/logging.h" +#include "libmv/multiview/homography.h" +#include "libmv/multiview/homography_parameterization.h" + +namespace libmv { +/** 2D Homography transformation estimation in the case that points are in + * euclidean coordinates. + * + * x = H y + * x and y vector must have the same direction, we could write + * crossproduct(|x|, * H * |y| ) = |0| + * + * | 0 -1 x2| |a b c| |y1| |0| + * | 1 0 -x1| * |d e f| * |y2| = |0| + * |-x2 x1 0| |g h 1| |1 | |0| + * + * That gives : + * + * (-d+x2*g)*y1 + (-e+x2*h)*y2 + -f+x2 |0| + * (a-x1*g)*y1 + (b-x1*h)*y2 + c-x1 = |0| + * (-x2*a+x1*d)*y1 + (-x2*b+x1*e)*y2 + -x2*c+x1*f |0| + */ +bool Homography2DFromCorrespondencesLinearEuc( + const Mat &x1, + const Mat &x2, + Mat3 *H, + double expected_precision) { + assert(2 == x1.rows()); + assert(4 <= x1.cols()); + assert(x1.rows() == x2.rows()); + assert(x1.cols() == x2.cols()); + + int n = x1.cols(); + MatX8 L = Mat::Zero(n * 3, 8); + Mat b = Mat::Zero(n * 3, 1); + for (int i = 0; i < n; ++i) { + int j = 3 * i; + L(j, 0) = x1(0, i); // a + L(j, 1) = x1(1, i); // b + L(j, 2) = 1.0; // c + L(j, 6) = -x2(0, i) * x1(0, i); // g + L(j, 7) = -x2(0, i) * x1(1, i); // h + b(j, 0) = x2(0, i); // i + + ++j; + L(j, 3) = x1(0, i); // d + L(j, 4) = x1(1, i); // e + L(j, 5) = 1.0; // f + L(j, 6) = -x2(1, i) * x1(0, i); // g + L(j, 7) = -x2(1, i) * x1(1, i); // h + b(j, 0) = x2(1, i); // i + + // This ensures better stability + // TODO(julien) make a lite version without this 3rd set + ++j; + L(j, 0) = x2(1, i) * x1(0, i); // a + L(j, 1) = x2(1, i) * x1(1, i); // b + L(j, 2) = x2(1, i); // c + L(j, 3) = -x2(0, i) * x1(0, i); // d + L(j, 4) = -x2(0, i) * x1(1, i); // e + L(j, 5) = -x2(0, i) ; // f + } + // Solve Lx=B + Vec h = L.fullPivLu().solve(b); + Homography2DNormalizedParameterization<double>::To(h, H); + if ((L * h).isApprox(b, expected_precision)) { + return true; + } else { + return false; + } +} + +/** 2D Homography transformation estimation in the case that points are in + * homogeneous coordinates. + * + * | 0 -x3 x2| |a b c| |y1| -x3*d+x2*g -x3*e+x2*h -x3*f+x2*1 |y1| (-x3*d+x2*g)*y1 (-x3*e+x2*h)*y2 (-x3*f+x2*1)*y3 |0| + * | x3 0 -x1| * |d e f| * |y2| = x3*a-x1*g x3*b-x1*h x3*c-x1*1 * |y2| = (x3*a-x1*g)*y1 (x3*b-x1*h)*y2 (x3*c-x1*1)*y3 = |0| + * |-x2 x1 0| |g h 1| |y3| -x2*a+x1*d -x2*b+x1*e -x2*c+x1*f |y3| (-x2*a+x1*d)*y1 (-x2*b+x1*e)*y2 (-x2*c+x1*f)*y3 |0| + * X = |a b c d e f g h|^t + */ +bool Homography2DFromCorrespondencesLinear(const Mat &x1, + const Mat &x2, + Mat3 *H, + double expected_precision) { + if (x1.rows() == 2) { + return Homography2DFromCorrespondencesLinearEuc(x1, x2, H, + expected_precision); + } + assert(3 == x1.rows()); + assert(4 <= x1.cols()); + assert(x1.rows() == x2.rows()); + assert(x1.cols() == x2.cols()); + + const int x = 0; + const int y = 1; + const int w = 2; + int n = x1.cols(); + MatX8 L = Mat::Zero(n * 3, 8); + Mat b = Mat::Zero(n * 3, 1); + for (int i = 0; i < n; ++i) { + int j = 3 * i; + L(j, 0) = x2(w, i) * x1(x, i);//a + L(j, 1) = x2(w, i) * x1(y, i);//b + L(j, 2) = x2(w, i) * x1(w, i);//c + L(j, 6) = -x2(x, i) * x1(x, i);//g + L(j, 7) = -x2(x, i) * x1(y, i);//h + b(j, 0) = x2(x, i) * x1(w, i); + + ++j; + L(j, 3) = x2(w, i) * x1(x, i);//d + L(j, 4) = x2(w, i) * x1(y, i);//e + L(j, 5) = x2(w, i) * x1(w, i);//f + L(j, 6) = -x2(y, i) * x1(x, i);//g + L(j, 7) = -x2(y, i) * x1(y, i);//h + b(j, 0) = x2(y, i) * x1(w, i); + + // This ensures better stability + ++j; + L(j, 0) = x2(y, i) * x1(x, i);//a + L(j, 1) = x2(y, i) * x1(y, i);//b + L(j, 2) = x2(y, i) * x1(w, i);//c + L(j, 3) = -x2(x, i) * x1(x, i);//d + L(j, 4) = -x2(x, i) * x1(y, i);//e + L(j, 5) = -x2(x, i) * x1(w, i);//f + } + // Solve Lx=B + Vec h = L.fullPivLu().solve(b); + if ((L * h).isApprox(b, expected_precision)) { + Homography2DNormalizedParameterization<double>::To(h, H); + return true; + } else { + return false; + } +} +/** + * x2 ~ A * x1 + * x2^t * Hi * A *x1 = 0 + * H1 = H2 = H3 = + * | 0 0 0 1| |-x2w| |0 0 0 0| | 0 | | 0 0 1 0| |-x2z| + * | 0 0 0 0| -> | 0 | |0 0 1 0| -> |-x2z| | 0 0 0 0| -> | 0 | + * | 0 0 0 0| | 0 | |0-1 0 0| | x2y| |-1 0 0 0| | x2x| + * |-1 0 0 0| | x2x| |0 0 0 0| | 0 | | 0 0 0 0| | 0 | + * H4 = H5 = H6 = + * |0 0 0 0| | 0 | | 0 1 0 0| |-x2y| |0 0 0 0| | 0 | + * |0 0 0 1| -> |-x2w| |-1 0 0 0| -> | x2x| |0 0 0 0| -> | 0 | + * |0 0 0 0| | 0 | | 0 0 0 0| | 0 | |0 0 0 1| |-x2w| + * |0-1 0 0| | x2y| | 0 0 0 0| | 0 | |0 0-1 0| | x2z| + * |a b c d| + * A = |e f g h| + * |i j k l| + * |m n o 1| + * + * x2^t * H1 * A *x1 = (-x2w*a +x2x*m )*x1x + (-x2w*b +x2x*n )*x1y + (-x2w*c +x2x*o )*x1z + (-x2w*d +x2x*1 )*x1w = 0 + * x2^t * H2 * A *x1 = (-x2z*e +x2y*i )*x1x + (-x2z*f +x2y*j )*x1y + (-x2z*g +x2y*k )*x1z + (-x2z*h +x2y*l )*x1w = 0 + * x2^t * H3 * A *x1 = (-x2z*a +x2x*i )*x1x + (-x2z*b +x2x*j )*x1y + (-x2z*c +x2x*k )*x1z + (-x2z*d +x2x*l )*x1w = 0 + * x2^t * H4 * A *x1 = (-x2w*e +x2y*m )*x1x + (-x2w*f +x2y*n )*x1y + (-x2w*g +x2y*o )*x1z + (-x2w*h +x2y*1 )*x1w = 0 + * x2^t * H5 * A *x1 = (-x2y*a +x2x*e )*x1x + (-x2y*b +x2x*f )*x1y + (-x2y*c +x2x*g )*x1z + (-x2y*d +x2x*h )*x1w = 0 + * x2^t * H6 * A *x1 = (-x2w*i +x2z*m )*x1x + (-x2w*j +x2z*n )*x1y + (-x2w*k +x2z*o )*x1z + (-x2w*l +x2z*1 )*x1w = 0 + * + * X = |a b c d e f g h i j k l m n o|^t +*/ +bool Homography3DFromCorrespondencesLinear(const Mat &x1, + const Mat &x2, + Mat4 *H, + double expected_precision) { + assert(4 == x1.rows()); + assert(5 <= x1.cols()); + assert(x1.rows() == x2.rows()); + assert(x1.cols() == x2.cols()); + const int x = 0; + const int y = 1; + const int z = 2; + const int w = 3; + int n = x1.cols(); + MatX15 L = Mat::Zero(n * 6, 15); + Mat b = Mat::Zero(n * 6, 1); + for (int i = 0; i < n; ++i) { + int j = 6 * i; + L(j, 0) = -x2(w, i) * x1(x, i);//a + L(j, 1) = -x2(w, i) * x1(y, i);//b + L(j, 2) = -x2(w, i) * x1(z, i);//c + L(j, 3) = -x2(w, i) * x1(w, i);//d + L(j,12) = x2(x, i) * x1(x, i);//m + L(j,13) = x2(x, i) * x1(y, i);//n + L(j,14) = x2(x, i) * x1(z, i);//o + b(j, 0) = -x2(x, i) * x1(w, i); + + ++j; + L(j, 4) = -x2(z, i) * x1(x, i);//e + L(j, 5) = -x2(z, i) * x1(y, i);//f + L(j, 6) = -x2(z, i) * x1(z, i);//g + L(j, 7) = -x2(z, i) * x1(w, i);//h + L(j, 8) = x2(y, i) * x1(x, i);//i + L(j, 9) = x2(y, i) * x1(y, i);//j + L(j,10) = x2(y, i) * x1(z, i);//k + L(j,11) = x2(y, i) * x1(w, i);//l + + ++j; + L(j, 0) = -x2(z, i) * x1(x, i);//a + L(j, 1) = -x2(z, i) * x1(y, i);//b + L(j, 2) = -x2(z, i) * x1(z, i);//c + L(j, 3) = -x2(z, i) * x1(w, i);//d + L(j, 8) = x2(x, i) * x1(x, i);//i + L(j, 9) = x2(x, i) * x1(y, i);//j + L(j,10) = x2(x, i) * x1(z, i);//k + L(j,11) = x2(x, i) * x1(w, i);//l + + ++j; + L(j, 4) = -x2(w, i) * x1(x, i);//e + L(j, 5) = -x2(w, i) * x1(y, i);//f + L(j, 6) = -x2(w, i) * x1(z, i);//g + L(j, 7) = -x2(w, i) * x1(w, i);//h + L(j,12) = x2(y, i) * x1(x, i);//m + L(j,13) = x2(y, i) * x1(y, i);//n + L(j,14) = x2(y, i) * x1(z, i);//o + b(j, 0) = -x2(y, i) * x1(w, i); + + ++j; + L(j, 0) = -x2(y, i) * x1(x, i);//a + L(j, 1) = -x2(y, i) * x1(y, i);//b + L(j, 2) = -x2(y, i) * x1(z, i);//c + L(j, 3) = -x2(y, i) * x1(w, i);//d + L(j, 4) = x2(x, i) * x1(x, i);//e + L(j, 5) = x2(x, i) * x1(y, i);//f + L(j, 6) = x2(x, i) * x1(z, i);//g + L(j, 7) = x2(x, i) * x1(w, i);//h + + ++j; + L(j, 8) = -x2(w, i) * x1(x, i);//i + L(j, 9) = -x2(w, i) * x1(y, i);//j + L(j,10) = -x2(w, i) * x1(z, i);//k + L(j,11) = -x2(w, i) * x1(w, i);//l + L(j,12) = x2(z, i) * x1(x, i);//m + L(j,13) = x2(z, i) * x1(y, i);//n + L(j,14) = x2(z, i) * x1(z, i);//o + b(j, 0) = -x2(z, i) * x1(w, i); + } + // Solve Lx=B + Vec h = L.fullPivLu().solve(b); + if ((L * h).isApprox(b, expected_precision)) { + Homography3DNormalizedParameterization<double>::To(h, H); + return true; + } else { + return false; + } +} +} // namespace libmv diff --git a/extern/libmv/libmv/multiview/homography.h b/extern/libmv/libmv/multiview/homography.h new file mode 100644 index 00000000000..786fd3df8ca --- /dev/null +++ b/extern/libmv/libmv/multiview/homography.h @@ -0,0 +1,84 @@ +// Copyright (c) 2011 libmv authors. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to +// deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. + +#ifndef LIBMV_MULTIVIEW_HOMOGRAPHY_H_ +#define LIBMV_MULTIVIEW_HOMOGRAPHY_H_ + +#include "libmv/numeric/numeric.h" + +namespace libmv { + +/** + * 2D homography transformation estimation. + * + * This function estimates the homography transformation from a list of 2D + * correspondences which represents either: + * + * - 3D points on a plane, with a general moving camera. + * - 3D points with a rotating camera (pure rotation). + * - 3D points + different planar projections + * + * \param x1 The first 2xN or 3xN matrix of euclidean or homogeneous points. + * \param x2 The second 2xN or 3xN matrix of euclidean or homogeneous points. + * \param H The 3x3 homography transformation matrix (8 dof) such that + * x2 = H * x1 with |a b c| + * H = |d e f| + * |g h 1| + * \param expected_precision The expected precision in order for instance + * to accept almost homography matrices. + * + * \return True if the transformation estimation has succeeded. + * \note There must be at least 4 non-colinear points. + */ +bool Homography2DFromCorrespondencesLinear(const Mat &x1, + const Mat &x2, + Mat3 *H, + double expected_precision = + EigenDouble::dummy_precision()); + +/** + * 3D Homography transformation estimation. + * + * This function can be used in order to estimate the homography transformation + * from a list of 3D correspondences. + * + * \param[in] x1 The first 4xN matrix of homogeneous points + * \param[in] x2 The second 4xN matrix of homogeneous points + * \param[out] H The 4x4 homography transformation matrix (15 dof) such that + * x2 = H * x1 with |a b c d| + * H = |e f g h| + * |i j k l| + * |m n o 1| + * \param[in] expected_precision The expected precision in order for instance + * to accept almost homography matrices. + * + * \return true if the transformation estimation has succeeded + * + * \note Need at least 5 non coplanar points + * \note Points coordinates must be in homogeneous coordinates + */ +bool Homography3DFromCorrespondencesLinear(const Mat &x1, + const Mat &x2, + Mat4 *H, + double expected_precision = + EigenDouble::dummy_precision()); +} // namespace libmv + +#endif // LIBMV_MULTIVIEW_HOMOGRAPHY_H_ diff --git a/extern/libmv/libmv/multiview/homography_parameterization.h b/extern/libmv/libmv/multiview/homography_parameterization.h new file mode 100644 index 00000000000..b31642eea15 --- /dev/null +++ b/extern/libmv/libmv/multiview/homography_parameterization.h @@ -0,0 +1,91 @@ +// Copyright (c) 2011 libmv authors. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to +// deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. + +#ifndef LIBMV_MULTIVIEW_HOMOGRAPHY_PARAMETERIZATION_H_ +#define LIBMV_MULTIVIEW_HOMOGRAPHY_PARAMETERIZATION_H_ + +#include "libmv/numeric/numeric.h" + +namespace libmv { + +/** A parameterization of the 2D homography matrix that uses 8 parameters so + * that the matrix is normalized (H(2,2) == 1). + * The homography matrix H is built from a list of 8 parameters (a, b,...g, h) + * as follows + * |a b c| + * H = |d e f| + * |g h 1| + */ +template<typename T = double> +class Homography2DNormalizedParameterization { + public: + typedef Eigen::Matrix<T, 8, 1> Parameters; // a, b, ... g, h + typedef Eigen::Matrix<T, 3, 3> Parameterized; // H + + /// Convert from the 8 parameters to a H matrix. + static void To(const Parameters &p, Parameterized *h) { + *h << p(0), p(1), p(2), + p(3), p(4), p(5), + p(6), p(7), 1.0; + } + + /// Convert from a H matrix to the 8 parameters. + static void From(const Parameterized &h, Parameters *p) { + *p << h(0, 0), h(0, 1), h(0, 2), + h(1, 0), h(1, 1), h(1, 2), + h(2, 0), h(2, 1); + } +}; + +/** A parameterization of the 2D homography matrix that uses 15 parameters so + * that the matrix is normalized (H(3,3) == 1). + * The homography matrix H is built from a list of 15 parameters (a, b,...n, o) + * as follows + * |a b c d| + * H = |e f g h| + * |i j k l| + * |m n o 1| + */ +template<typename T = double> +class Homography3DNormalizedParameterization { + public: + typedef Eigen::Matrix<T, 15, 1> Parameters; // a, b, ... n, o + typedef Eigen::Matrix<T, 4, 4> Parameterized; // H + + /// Convert from the 15 parameters to a H matrix. + static void To(const Parameters &p, Parameterized *h) { + *h << p(0), p(1), p(2), p(3), + p(4), p(5), p(6), p(7), + p(8), p(9), p(10), p(11), + p(12), p(13), p(14), 1.0; + } + + /// Convert from a H matrix to the 15 parameters. + static void From(const Parameterized &h, Parameters *p) { + *p << h(0, 0), h(0, 1), h(0, 2), h(0, 3), + h(1, 0), h(1, 1), h(1, 2), h(1, 3), + h(2, 0), h(2, 1), h(2, 2), h(2, 3), + h(3, 0), h(3, 1), h(3, 2); + } +}; + +} // namespace libmv + +#endif // LIBMV_MULTIVIEW_HOMOGRAPHY_PARAMETERIZATION_H_ diff --git a/extern/libmv/libmv/tracking/esm_region_tracker.cc b/extern/libmv/libmv/tracking/esm_region_tracker.cc index 221fa4d081b..a8dc46d439b 100644 --- a/extern/libmv/libmv/tracking/esm_region_tracker.cc +++ b/extern/libmv/libmv/tracking/esm_region_tracker.cc @@ -29,6 +29,7 @@ #include "libmv/image/correlation.h" #include "libmv/image/sample.h" #include "libmv/numeric/numeric.h" +#include "libmv/tracking/track_region.h" namespace libmv { @@ -72,6 +73,44 @@ bool EsmRegionTracker::Track(const FloatImage &image1, return false; } + // XXX + // TODO(keir): Delete the block between the XXX's once the planar tracker is + // integrated into blender. + // + // For now, to test, replace the ESM tracker with the Ceres tracker in + // translation mode. In the future, this should get removed and alloed to + // co-exist, since Ceres is not as fast as the ESM implementation since it + // specializes for translation. + double xx1[4], yy1[4]; + double xx2[4], yy2[4]; + // Clockwise winding, starting from the "origin" (top-left). + xx1[0] = xx2[0] = x1 - half_window_size; + yy1[0] = yy2[0] = y1 - half_window_size; + + xx1[1] = xx2[1] = x1 + half_window_size; + yy1[1] = yy2[1] = y1 - half_window_size; + + xx1[2] = xx2[2] = x1 + half_window_size; + yy1[2] = yy2[2] = y1 + half_window_size; + + xx1[3] = xx2[3] = x1 - half_window_size; + yy1[3] = yy2[3] = y1 + half_window_size; + + TrackRegionOptions options; + options.mode = TrackRegionOptions::TRANSLATION; + options.max_iterations = 20; + options.sigma = sigma; + options.use_esm = true; + + TrackRegionResult result; + TrackRegion(image1, image2, xx1, yy1, options, xx2, yy2, &result); + + *x2 = xx2[0] + half_window_size; + *y2 = yy2[0] + half_window_size; + + return true; + + // XXX int width = 2 * half_window_size + 1; // TODO(keir): Avoid recomputing gradients for e.g. the pyramid tracker. diff --git a/extern/libmv/libmv/tracking/track_region.cc b/extern/libmv/libmv/tracking/track_region.cc new file mode 100644 index 00000000000..e65ead50c80 --- /dev/null +++ b/extern/libmv/libmv/tracking/track_region.cc @@ -0,0 +1,1391 @@ +// Copyright (c) 2012 libmv authors. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to +// deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. +// +// Author: mierle@google.com (Keir Mierle) +// +// TODO(keir): While this tracking code works rather well, it has some +// outragous inefficiencies. There is probably a 5-10x speedup to be had if a +// smart coder went through the TODO's and made the suggested performance +// enhancements. + +// Necessary for M_E when building with MSVC. +#define _USE_MATH_DEFINES + +#include "libmv/tracking/track_region.h" + +#include <Eigen/SVD> +#include <Eigen/QR> +#include <iostream> +#include "ceres/ceres.h" +#include "libmv/logging/logging.h" +#include "libmv/image/image.h" +#include "libmv/image/sample.h" +#include "libmv/image/convolve.h" +#include "libmv/multiview/homography.h" +#include "libmv/numeric/numeric.h" + +namespace libmv { + +using ceres::Jet; +using ceres::JetOps; +using ceres::Chain; + +TrackRegionOptions::TrackRegionOptions() + : mode(TRANSLATION), + minimum_correlation(0), + max_iterations(20), + use_esm(true), + use_brute_initialization(true), + use_normalized_intensities(false), + sigma(0.9), + num_extra_points(0), + regularization_coefficient(0.0), + image1_mask(NULL) { +} + +namespace { + +// TODO(keir): Consider adding padding. +template<typename T> +bool InBounds(const FloatImage &image, + const T &x, + const T &y) { + return 0.0 <= x && x < image.Width() && + 0.0 <= y && y < image.Height(); +} + +bool AllInBounds(const FloatImage &image, + const double *x, + const double *y) { + for (int i = 0; i < 4; ++i) { + if (!InBounds(image, x[i], y[i])) { + return false; + } + } + return true; +} + +// Sample the image at position (x, y) but use the gradient, if present, to +// propagate derivatives from x and y. This is needed to integrate the numeric +// image gradients with Ceres's autodiff framework. +template<typename T> +static T SampleWithDerivative(const FloatImage &image_and_gradient, + const T &x, + const T &y) { + float scalar_x = JetOps<T>::GetScalar(x); + float scalar_y = JetOps<T>::GetScalar(y); + + // Note that sample[1] and sample[2] will be uninitialized in the scalar + // case, but that is not an issue because the Chain::Rule below will not read + // the uninitialized values. + float sample[3]; + if (JetOps<T>::IsScalar()) { + // For the scalar case, only sample the image. + sample[0] = SampleLinear(image_and_gradient, scalar_y, scalar_x, 0); + } else { + // For the derivative case, sample the gradient as well. + SampleLinear(image_and_gradient, scalar_y, scalar_x, sample); + } + T xy[2] = { x, y }; + return Chain<float, 2, T>::Rule(sample[0], sample + 1, xy); +} + +template<typename Warp> +class BoundaryCheckingCallback : public ceres::IterationCallback { + public: + BoundaryCheckingCallback(const FloatImage& image2, + const Warp &warp, + const double *x1, const double *y1) + : image2_(image2), warp_(warp), x1_(x1), y1_(y1) {} + + virtual ceres::CallbackReturnType operator()( + const ceres::IterationSummary& summary) { + // Warp the original 4 points with the current warp into image2. + double x2[4]; + double y2[4]; + for (int i = 0; i < 4; ++i) { + warp_.Forward(warp_.parameters, x1_[i], y1_[i], x2 + i, y2 + i); + } + // Enusre they are all in bounds. + if (!AllInBounds(image2_, x2, y2)) { + return ceres::SOLVER_ABORT; + } + return ceres::SOLVER_CONTINUE; + } + + private: + const FloatImage &image2_; + const Warp &warp_; + const double *x1_; + const double *y1_; +}; + +template<typename Warp> +class PixelDifferenceCostFunctor { + public: + PixelDifferenceCostFunctor(const TrackRegionOptions &options, + const FloatImage &image_and_gradient1, + const FloatImage &image_and_gradient2, + const Mat3 &canonical_to_image1, + int num_samples_x, + int num_samples_y, + const Warp &warp) + : options_(options), + image_and_gradient1_(image_and_gradient1), + image_and_gradient2_(image_and_gradient2), + canonical_to_image1_(canonical_to_image1), + num_samples_x_(num_samples_x), + num_samples_y_(num_samples_y), + warp_(warp), + pattern_and_gradient_(num_samples_y_, num_samples_x_, 3), + pattern_positions_(num_samples_y_, num_samples_x_, 2), + pattern_mask_(num_samples_y_, num_samples_x_, 1) { + ComputeCanonicalPatchAndNormalizer(); + } + + void ComputeCanonicalPatchAndNormalizer() { + src_mean_ = 0.0; + double num_samples = 0.0; + for (int r = 0; r < num_samples_y_; ++r) { + for (int c = 0; c < num_samples_x_; ++c) { + // Compute the position; cache it. + Vec3 image_position = canonical_to_image1_ * Vec3(c, r, 1); + image_position /= image_position(2); + pattern_positions_(r, c, 0) = image_position(0); + pattern_positions_(r, c, 1) = image_position(1); + + // Sample the pattern and gradients. + SampleLinear(image_and_gradient1_, + image_position(1), // SampleLinear is r, c. + image_position(0), + &pattern_and_gradient_(r, c, 0)); + + // Sample sample the mask. + double mask_value = 1.0; + if (options_.image1_mask != NULL) { + SampleLinear(*options_.image1_mask, + image_position(1), // SampleLinear is r, c. + image_position(0), + &pattern_mask_(r, c, 0)); + mask_value = pattern_mask_(r, c); + } + src_mean_ += pattern_and_gradient_(r, c, 0) * mask_value; + num_samples += mask_value; + } + } + src_mean_ /= num_samples; + } + + template<typename T> + bool operator()(const T *warp_parameters, T *residuals) const { + if (options_.image1_mask != NULL) { + VLOG(2) << "Using a mask."; + } + for (int i = 0; i < Warp::NUM_PARAMETERS; ++i) { + VLOG(2) << "warp_parameters[" << i << "]: " << warp_parameters[i]; + } + + T dst_mean = T(1.0); + if (options_.use_normalized_intensities) { + ComputeNormalizingCoefficient(warp_parameters, + &dst_mean); + } + + int cursor = 0; + for (int r = 0; r < num_samples_y_; ++r) { + for (int c = 0; c < num_samples_x_; ++c) { + // Use the pre-computed image1 position. + Vec2 image1_position(pattern_positions_(r, c, 0), + pattern_positions_(r, c, 1)); + + // Sample the mask early; if it's zero, this pixel has no effect. This + // allows early bailout from the expensive sampling that happens below. + // + // Note that partial masks are not short circuited. To see why short + // circuiting produces bitwise-exact same results, consider that the + // residual for each pixel is + // + // residual = mask * (src - dst) , + // + // and for jets, multiplying by a scalar multiplies the derivative + // components by the scalar as well. Therefore, if the mask is exactly + // zero, then so too will the final residual and derivatives. + double mask_value = 1.0; + if (options_.image1_mask != NULL) { + mask_value = pattern_mask_(r, c); + if (mask_value == 0.0) { + residuals[cursor++] = T(0.0); + continue; + } + } + + // Compute the location of the destination pixel. + T image2_position[2]; + warp_.Forward(warp_parameters, + T(image1_position[0]), + T(image1_position[1]), + &image2_position[0], + &image2_position[1]); + + // Sample the destination, propagating derivatives. + T dst_sample = SampleWithDerivative(image_and_gradient2_, + image2_position[0], + image2_position[1]); + + // Sample the source. This is made complicated by ESM mode. + T src_sample; + if (options_.use_esm && !JetOps<T>::IsScalar()) { + // In ESM mode, the derivative of the source is also taken into + // account. This changes the linearization in a way that causes + // better convergence. Copy the derivative of the warp parameters + // onto the jets for the image1 position. This is the ESM hack. + T image1_position_jet[2] = { + image2_position[0], // Order is x, y. This matches the + image2_position[1] // derivative order in the patch. + }; + JetOps<T>::SetScalar(image1_position[0], image1_position_jet + 0); + JetOps<T>::SetScalar(image1_position[1], image1_position_jet + 1); + + // Now that the image1 positions have the jets applied from the + // image2 position (the ESM hack), chain the image gradients to + // obtain a sample with the derivative with respect to the warp + // parameters attached. + src_sample = Chain<float, 2, T>::Rule(pattern_and_gradient_(r, c), + &pattern_and_gradient_(r, c, 1), + image1_position_jet); + + // The jacobians for these should be averaged. Due to the subtraction + // below, flip the sign of the src derivative so that the effect + // after subtraction of the jets is that they are averaged. + JetOps<T>::ScaleDerivative(-0.5, &src_sample); + JetOps<T>::ScaleDerivative(0.5, &dst_sample); + } else { + // This is the traditional, forward-mode KLT solution. + src_sample = T(pattern_and_gradient_(r, c)); + } + + // Normalize the samples by the mean values of each signal. The typical + // light model assumes multiplicative intensity changes with changing + // light, so this is a reasonable choice. Note that dst_mean has + // derivative information attached thanks to autodiff. + if (options_.use_normalized_intensities) { + src_sample /= T(src_mean_); + dst_sample /= dst_mean; + } + + // The difference is the error. + T error = src_sample - dst_sample; + + // Weight the error by the mask, if one is present. + if (options_.image1_mask != NULL) { + error *= T(mask_value); + } + residuals[cursor++] = error; + } + } + return true; + } + + // For normalized matching, the average and + template<typename T> + void ComputeNormalizingCoefficient(const T *warp_parameters, + T *dst_mean) const { + + *dst_mean = T(0.0); + double num_samples = 0.0; + for (int r = 0; r < num_samples_y_; ++r) { + for (int c = 0; c < num_samples_x_; ++c) { + // Use the pre-computed image1 position. + Vec2 image1_position(pattern_positions_(r, c, 0), + pattern_positions_(r, c, 1)); + + // Sample the mask early; if it's zero, this pixel has no effect. This + // allows early bailout from the expensive sampling that happens below. + double mask_value = 1.0; + if (options_.image1_mask != NULL) { + mask_value = pattern_mask_(r, c); + if (mask_value == 0.0) { + continue; + } + } + + // Compute the location of the destination pixel. + T image2_position[2]; + warp_.Forward(warp_parameters, + T(image1_position[0]), + T(image1_position[1]), + &image2_position[0], + &image2_position[1]); + + + // Sample the destination, propagating derivatives. + // TODO(keir): This accumulation can, surprisingly, be done as a + // pre-pass by using integral images. This is complicated by the need + // to store the jets in the integral image, but it is possible. + T dst_sample = SampleWithDerivative(image_and_gradient2_, + image2_position[0], + image2_position[1]); + + // Weight the sample by the mask, if one is present. + if (options_.image1_mask != NULL) { + dst_sample *= T(mask_value); + } + + *dst_mean += dst_sample; + num_samples += mask_value; + } + } + *dst_mean /= T(num_samples); + LG << "Normalization for dst:" << *dst_mean; + } + + // TODO(keir): Consider also computing the cost here. + double PearsonProductMomentCorrelationCoefficient( + const double *warp_parameters) const { + for (int i = 0; i < Warp::NUM_PARAMETERS; ++i) { + VLOG(2) << "Correlation warp_parameters[" << i << "]: " + << warp_parameters[i]; + } + + // The single-pass PMCC computation is somewhat numerically unstable, but + // it's sufficient for the tracker. + double sX = 0, sY = 0, sXX = 0, sYY = 0, sXY = 0; + + // Due to masking, it's important to account for fractional samples. + // For example, samples with a 50% mask are counted as a half sample. + double num_samples = 0; + + for (int r = 0; r < num_samples_y_; ++r) { + for (int c = 0; c < num_samples_x_; ++c) { + // Use the pre-computed image1 position. + Vec2 image1_position(pattern_positions_(r, c, 0), + pattern_positions_(r, c, 1)); + + double mask_value = 1.0; + if (options_.image1_mask != NULL) { + mask_value = pattern_mask_(r, c); + if (mask_value == 0.0) { + continue; + } + } + + // Compute the location of the destination pixel. + double image2_position[2]; + warp_.Forward(warp_parameters, + image1_position[0], + image1_position[1], + &image2_position[0], + &image2_position[1]); + + double x = pattern_and_gradient_(r, c); + double y = SampleLinear(image_and_gradient2_, + image2_position[1], // SampleLinear is r, c. + image2_position[0]); + + // Weight the signals by the mask, if one is present. + if (options_.image1_mask != NULL) { + x *= mask_value; + y *= mask_value; + num_samples += mask_value; + } else { + num_samples++; + } + sX += x; + sY += y; + sXX += x*x; + sYY += y*y; + sXY += x*y; + } + } + // Normalize. + sX /= num_samples; + sY /= num_samples; + sXX /= num_samples; + sYY /= num_samples; + sXY /= num_samples; + + double var_x = sXX - sX*sX; + double var_y = sYY - sY*sY; + double covariance_xy = sXY - sX*sY; + + double correlation = covariance_xy / sqrt(var_x * var_y); + LG << "Covariance xy: " << covariance_xy + << ", var 1: " << var_x << ", var 2: " << var_y + << ", correlation: " << correlation; + return correlation; + } + + private: + const TrackRegionOptions &options_; + const FloatImage &image_and_gradient1_; + const FloatImage &image_and_gradient2_; + const Mat3 &canonical_to_image1_; + int num_samples_x_; + int num_samples_y_; + const Warp &warp_; + double src_mean_; + FloatImage pattern_and_gradient_; + + // This contains the position from where the cached pattern samples were + // taken from. This is also used to warp from src to dest without going from + // canonical pixels to src first. + FloatImage pattern_positions_; + + FloatImage pattern_mask_; +}; + +template<typename Warp> +class WarpRegularizingCostFunctor { + public: + WarpRegularizingCostFunctor(const TrackRegionOptions &options, + const double *x1, + const double *y1, + const double *x2_original, + const double *y2_original, + const Warp &warp) + : options_(options), + x1_(x1), + y1_(y1), + x2_original_(x2_original), + y2_original_(y2_original), + warp_(warp) { + // Compute the centroid of the first guess quad. + // TODO(keir): Use Quad class here. + original_centroid_[0] = 0.0; + original_centroid_[1] = 0.0; + for (int i = 0; i < 4; ++i) { + original_centroid_[0] += x2_original[i]; + original_centroid_[1] += y2_original[i]; + } + original_centroid_[0] /= 4; + original_centroid_[1] /= 4; + } + + template<typename T> + bool operator()(const T *warp_parameters, T *residuals) const { + T dst_centroid[2] = { T(0.0), T(0.0) }; + for (int i = 0; i < 4; ++i) { + T image1_position[2] = { T(x1_[i]), T(y1_[i]) }; + T image2_position[2]; + warp_.Forward(warp_parameters, + T(x1_[i]), + T(y1_[i]), + &image2_position[0], + &image2_position[1]); + + // Subtract the positions. Note that this ignores the centroids. + residuals[2 * i + 0] = image2_position[0] - image1_position[0]; + residuals[2 * i + 1] = image2_position[1] - image1_position[1]; + + // Accumulate the dst centroid. + dst_centroid[0] += image2_position[0]; + dst_centroid[1] += image2_position[1]; + } + dst_centroid[0] /= T(4.0); + dst_centroid[1] /= T(4.0); + + // Adjust for the centroids. + for (int i = 0; i < 4; ++i) { + residuals[2 * i + 0] += T(original_centroid_[0]) - dst_centroid[0]; + residuals[2 * i + 1] += T(original_centroid_[1]) - dst_centroid[1]; + } + + // Reweight the residuals. + for (int i = 0; i < 8; ++i) { + residuals[i] *= T(options_.regularization_coefficient); + } + + return true; + } + + const TrackRegionOptions &options_; + const double *x1_; + const double *y1_; + const double *x2_original_; + const double *y2_original_; + double original_centroid_[2]; + const Warp &warp_; +}; + +// Compute the warp from rectangular coordinates, where one corner is the +// origin, and the opposite corner is at (num_samples_x, num_samples_y). +Mat3 ComputeCanonicalHomography(const double *x1, + const double *y1, + int num_samples_x, + int num_samples_y) { + Mat canonical(2, 4); + canonical << 0, num_samples_x, num_samples_x, 0, + 0, 0, num_samples_y, num_samples_y; + + Mat xy1(2, 4); + xy1 << x1[0], x1[1], x1[2], x1[3], + y1[0], y1[1], y1[2], y1[3]; + + Mat3 H; + if (!Homography2DFromCorrespondencesLinear(canonical, xy1, &H, 1e-12)) { + LG << "Couldn't construct homography."; + } + return H; +} + +class Quad { + public: + Quad(const double *x, const double *y) : x_(x), y_(y) { + // Compute the centroid and store it. + centroid_ = Vec2(0.0, 0.0); + for (int i = 0; i < 4; ++i) { + centroid_ += Vec2(x_[i], y_[i]); + } + centroid_ /= 4.0; + } + + // The centroid of the four points representing the quad. + const Vec2& Centroid() const { + return centroid_; + } + + // The average magnitude of the four points relative to the centroid. + double Scale() const { + double scale = 0.0; + for (int i = 0; i < 4; ++i) { + scale += (Vec2(x_[i], y_[i]) - Centroid()).norm(); + } + return scale / 4.0; + } + + Vec2 CornerRelativeToCentroid(int i) const { + return Vec2(x_[i], y_[i]) - centroid_; + } + + private: + const double *x_; + const double *y_; + Vec2 centroid_; +}; + +struct TranslationWarp { + TranslationWarp(const double *x1, const double *y1, + const double *x2, const double *y2) { + Vec2 t = Quad(x2, y2).Centroid() - Quad(x1, y1).Centroid(); + parameters[0] = t[0]; + parameters[1] = t[1]; + } + + template<typename T> + void Forward(const T *warp_parameters, + const T &x1, const T& y1, T *x2, T* y2) const { + *x2 = x1 + warp_parameters[0]; + *y2 = y1 + warp_parameters[1]; + } + + // Translation x, translation y. + enum { NUM_PARAMETERS = 2 }; + double parameters[NUM_PARAMETERS]; +}; + +struct TranslationScaleWarp { + TranslationScaleWarp(const double *x1, const double *y1, + const double *x2, const double *y2) + : q1(x1, y1) { + Quad q2(x2, y2); + + // The difference in centroids is the best guess for translation. + Vec2 t = q2.Centroid() - q1.Centroid(); + parameters[0] = t[0]; + parameters[1] = t[1]; + + // The difference in scales is the estimate for the scale. + parameters[2] = 1.0 - q2.Scale() / q1.Scale(); + } + + // The strange way of parameterizing the translation and scaling is to make + // the knobs that the optimizer sees easy to adjust. This is less important + // for the scaling case than the rotation case. + template<typename T> + void Forward(const T *warp_parameters, + const T &x1, const T& y1, T *x2, T* y2) const { + // Make the centroid of Q1 the origin. + const T x1_origin = x1 - q1.Centroid()(0); + const T y1_origin = y1 - q1.Centroid()(1); + + // Scale uniformly about the origin. + const T scale = 1.0 + warp_parameters[2]; + const T x1_origin_scaled = scale * x1_origin; + const T y1_origin_scaled = scale * y1_origin; + + // Translate back into the space of Q1 (but scaled). + const T x1_scaled = x1_origin_scaled + q1.Centroid()(0); + const T y1_scaled = y1_origin_scaled + q1.Centroid()(1); + + // Translate into the space of Q2. + *x2 = x1_scaled + warp_parameters[0]; + *y2 = y1_scaled + warp_parameters[1]; + } + + // Translation x, translation y, scale. + enum { NUM_PARAMETERS = 3 }; + double parameters[NUM_PARAMETERS]; + + Quad q1; +}; + +// Assumes the given points are already zero-centroid and the same size. +Mat2 OrthogonalProcrustes(const Mat2 &correlation_matrix) { + Eigen::JacobiSVD<Mat2> svd(correlation_matrix, + Eigen::ComputeFullU | Eigen::ComputeFullV); + return svd.matrixV() * svd.matrixU().transpose(); +} + +struct TranslationRotationWarp { + TranslationRotationWarp(const double *x1, const double *y1, + const double *x2, const double *y2) + : q1(x1, y1) { + Quad q2(x2, y2); + + // The difference in centroids is the best guess for translation. + Vec2 t = q2.Centroid() - q1.Centroid(); + parameters[0] = t[0]; + parameters[1] = t[1]; + + // Obtain the rotation via orthorgonal procrustes. + Mat2 correlation_matrix; + for (int i = 0; i < 4; ++i) { + correlation_matrix += q1.CornerRelativeToCentroid(i) * + q2.CornerRelativeToCentroid(i).transpose(); + } + Mat2 R = OrthogonalProcrustes(correlation_matrix); + parameters[2] = atan2(R(1, 0), R(0, 0)); + + LG << "Correlation_matrix:\n" << correlation_matrix; + LG << "R:\n" << R; + LG << "Theta:" << parameters[2]; + } + + // The strange way of parameterizing the translation and rotation is to make + // the knobs that the optimizer sees easy to adjust. The reason is that while + // it is always the case that it is possible to express composed rotations + // and translations as a single translation and rotation, the numerical + // values needed for the composition are often large in magnitude. This is + // enough to throw off any minimizer, since it must do the equivalent of + // compose rotations and translations. + // + // Instead, use the parameterization below that offers a parameterization + // that exposes the degrees of freedom in a way amenable to optimization. + template<typename T> + void Forward(const T *warp_parameters, + const T &x1, const T& y1, T *x2, T* y2) const { + // Make the centroid of Q1 the origin. + const T x1_origin = x1 - q1.Centroid()(0); + const T y1_origin = y1 - q1.Centroid()(1); + + // Rotate about the origin (i.e. centroid of Q1). + const T theta = warp_parameters[2]; + const T costheta = cos(theta); + const T sintheta = sin(theta); + const T x1_origin_rotated = costheta * x1_origin - sintheta * y1_origin; + const T y1_origin_rotated = sintheta * x1_origin + costheta * y1_origin; + + // Translate back into the space of Q1 (but scaled). + const T x1_rotated = x1_origin_rotated + q1.Centroid()(0); + const T y1_rotated = y1_origin_rotated + q1.Centroid()(1); + + // Translate into the space of Q2. + *x2 = x1_rotated + warp_parameters[0]; + *y2 = y1_rotated + warp_parameters[1]; + } + + // Translation x, translation y, rotation about the center of Q1 degrees. + enum { NUM_PARAMETERS = 3 }; + double parameters[NUM_PARAMETERS]; + + Quad q1; +}; + +struct TranslationRotationScaleWarp { + TranslationRotationScaleWarp(const double *x1, const double *y1, + const double *x2, const double *y2) + : q1(x1, y1) { + Quad q2(x2, y2); + + // The difference in centroids is the best guess for translation. + Vec2 t = q2.Centroid() - q1.Centroid(); + parameters[0] = t[0]; + parameters[1] = t[1]; + + // The difference in scales is the estimate for the scale. + parameters[2] = 1.0 - q2.Scale() / q1.Scale(); + + // Obtain the rotation via orthorgonal procrustes. + Mat2 correlation_matrix; + for (int i = 0; i < 4; ++i) { + correlation_matrix += q1.CornerRelativeToCentroid(i) * + q2.CornerRelativeToCentroid(i).transpose(); + } + Mat2 R = OrthogonalProcrustes(correlation_matrix); + parameters[3] = atan2(R(1, 0), R(0, 0)); + + LG << "Correlation_matrix:\n" << correlation_matrix; + LG << "R:\n" << R; + LG << "Theta:" << parameters[3]; + } + + // The strange way of parameterizing the translation and rotation is to make + // the knobs that the optimizer sees easy to adjust. The reason is that while + // it is always the case that it is possible to express composed rotations + // and translations as a single translation and rotation, the numerical + // values needed for the composition are often large in magnitude. This is + // enough to throw off any minimizer, since it must do the equivalent of + // compose rotations and translations. + // + // Instead, use the parameterization below that offers a parameterization + // that exposes the degrees of freedom in a way amenable to optimization. + template<typename T> + void Forward(const T *warp_parameters, + const T &x1, const T& y1, T *x2, T* y2) const { + // Make the centroid of Q1 the origin. + const T x1_origin = x1 - q1.Centroid()(0); + const T y1_origin = y1 - q1.Centroid()(1); + + // Rotate about the origin (i.e. centroid of Q1). + const T theta = warp_parameters[3]; + const T costheta = cos(theta); + const T sintheta = sin(theta); + const T x1_origin_rotated = costheta * x1_origin - sintheta * y1_origin; + const T y1_origin_rotated = sintheta * x1_origin + costheta * y1_origin; + + // Scale uniformly about the origin. + const T scale = 1.0 + warp_parameters[2]; + const T x1_origin_rotated_scaled = scale * x1_origin_rotated; + const T y1_origin_rotated_scaled = scale * y1_origin_rotated; + + // Translate back into the space of Q1 (but scaled and rotated). + const T x1_rotated_scaled = x1_origin_rotated_scaled + q1.Centroid()(0); + const T y1_rotated_scaled = y1_origin_rotated_scaled + q1.Centroid()(1); + + // Translate into the space of Q2. + *x2 = x1_rotated_scaled + warp_parameters[0]; + *y2 = y1_rotated_scaled + warp_parameters[1]; + } + + // Translation x, translation y, rotation about the center of Q1 degrees, + // scale. + enum { NUM_PARAMETERS = 4 }; + double parameters[NUM_PARAMETERS]; + + Quad q1; +}; + +struct AffineWarp { + AffineWarp(const double *x1, const double *y1, + const double *x2, const double *y2) + : q1(x1, y1) { + Quad q2(x2, y2); + + // The difference in centroids is the best guess for translation. + Vec2 t = q2.Centroid() - q1.Centroid(); + parameters[0] = t[0]; + parameters[1] = t[1]; + + // Estimate the four affine parameters with the usual least squares. + Mat Q1(8, 4); + Vec Q2(8); + for (int i = 0; i < 4; ++i) { + Vec2 v1 = q1.CornerRelativeToCentroid(i); + Vec2 v2 = q2.CornerRelativeToCentroid(i); + + Q1.row(2 * i + 0) << v1[0], v1[1], 0, 0 ; + Q1.row(2 * i + 1) << 0, 0, v1[0], v1[1]; + + Q2(2 * i + 0) = v2[0]; + Q2(2 * i + 1) = v2[1]; + } + + // TODO(keir): Check solution quality. + Vec4 a = Q1.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(Q2); + parameters[2] = a[0]; + parameters[3] = a[1]; + parameters[4] = a[2]; + parameters[5] = a[3]; + + LG << "a:" << a.transpose(); + LG << "t:" << t.transpose(); + } + + // See comments in other parameterizations about why the centroid is used. + template<typename T> + void Forward(const T *p, const T &x1, const T& y1, T *x2, T* y2) const { + // Make the centroid of Q1 the origin. + const T x1_origin = x1 - q1.Centroid()(0); + const T y1_origin = y1 - q1.Centroid()(1); + + // Apply the affine transformation. + const T x1_origin_affine = p[2] * x1_origin + p[3] * y1_origin; + const T y1_origin_affine = p[4] * x1_origin + p[5] * y1_origin; + + // Translate back into the space of Q1 (but affine transformed). + const T x1_affine = x1_origin_affine + q1.Centroid()(0); + const T y1_affine = y1_origin_affine + q1.Centroid()(1); + + // Translate into the space of Q2. + *x2 = x1_affine + p[0]; + *y2 = y1_affine + p[1]; + } + + // Translation x, translation y, rotation about the center of Q1 degrees, + // scale. + enum { NUM_PARAMETERS = 6 }; + double parameters[NUM_PARAMETERS]; + + Quad q1; +}; + +struct HomographyWarp { + HomographyWarp(const double *x1, const double *y1, + const double *x2, const double *y2) { + Mat quad1(2, 4); + quad1 << x1[0], x1[1], x1[2], x1[3], + y1[0], y1[1], y1[2], y1[3]; + + Mat quad2(2, 4); + quad2 << x2[0], x2[1], x2[2], x2[3], + y2[0], y2[1], y2[2], y2[3]; + + Mat3 H; + if (!Homography2DFromCorrespondencesLinear(quad1, quad2, &H, 1e-12)) { + LG << "Couldn't construct homography."; + } + + // Assume H(2, 2) != 0, and fix scale at H(2, 2) == 1.0. + H /= H(2, 2); + + // Assume H is close to identity, so subtract out the diagonal. + H(0, 0) -= 1.0; + H(1, 1) -= 1.0; + + CHECK_NE(H(2, 2), 0.0) << H; + for (int i = 0; i < 8; ++i) { + parameters[i] = H(i / 3, i % 3); + LG << "Parameters[" << i << "]: " << parameters[i]; + } + } + + template<typename T> + static void Forward(const T *p, + const T &x1, const T& y1, T *x2, T* y2) { + // Homography warp with manual 3x3 matrix multiply. + const T xx2 = (1.0 + p[0]) * x1 + p[1] * y1 + p[2]; + const T yy2 = p[3] * x1 + (1.0 + p[4]) * y1 + p[5]; + const T zz2 = p[6] * x1 + p[7] * y1 + 1.0; + *x2 = xx2 / zz2; + *y2 = yy2 / zz2; + } + + enum { NUM_PARAMETERS = 8 }; + double parameters[NUM_PARAMETERS]; +}; + +// Determine the number of samples to use for x and y. Quad winding goes: +// +// 0 1 +// 3 2 +// +// The idea is to take the maximum x or y distance. This may be oversampling. +// TODO(keir): Investigate the various choices; perhaps average is better? +void PickSampling(const double *x1, const double *y1, + const double *x2, const double *y2, + int *num_samples_x, int *num_samples_y) { + Vec2 a0(x1[0], y1[0]); + Vec2 a1(x1[1], y1[1]); + Vec2 a2(x1[2], y1[2]); + Vec2 a3(x1[3], y1[3]); + + Vec2 b0(x1[0], y1[0]); + Vec2 b1(x1[1], y1[1]); + Vec2 b2(x1[2], y1[2]); + Vec2 b3(x1[3], y1[3]); + + double x_dimensions[4] = { + (a1 - a0).norm(), + (a3 - a2).norm(), + (b1 - b0).norm(), + (b3 - b2).norm() + }; + + double y_dimensions[4] = { + (a3 - a0).norm(), + (a1 - a2).norm(), + (b3 - b0).norm(), + (b1 - b2).norm() + }; + const double kScaleFactor = 1.0; + *num_samples_x = static_cast<int>( + kScaleFactor * *std::max_element(x_dimensions, x_dimensions + 4)); + *num_samples_y = static_cast<int>( + kScaleFactor * *std::max_element(y_dimensions, y_dimensions + 4)); + LG << "Automatic num_samples_x: " << *num_samples_x + << ", num_samples_y: " << *num_samples_y; +} + +bool SearchAreaTooBigForDescent(const FloatImage &image2, + const double *x2, const double *y2) { + // TODO(keir): Check the bounds and enable only when it makes sense. + return true; +} + +bool PointOnRightHalfPlane(const Vec2 &a, const Vec2 &b, double x, double y) { + Vec2 ba = b - a; + return ((Vec2(x, y) - b).transpose() * Vec2(-ba.y(), ba.x())) > 0; +} + +// Determine if a point is in a quad. The quad is arranged as: +// +// +-------> x +// | +// | a0------a1 +// | | | +// | | | +// | | | +// | a3------a2 +// v +// y +// +// The implementation does up to four half-plane comparisons. +bool PointInQuad(const double *xs, const double *ys, double x, double y) { + Vec2 a0(xs[0], ys[0]); + Vec2 a1(xs[1], ys[1]); + Vec2 a2(xs[2], ys[2]); + Vec2 a3(xs[3], ys[3]); + + return PointOnRightHalfPlane(a0, a1, x, y) && + PointOnRightHalfPlane(a1, a2, x, y) && + PointOnRightHalfPlane(a2, a3, x, y) && + PointOnRightHalfPlane(a3, a0, x, y); +} + +// This makes it possible to map between Eigen float arrays and FloatImage +// without using comparisons. +typedef Eigen::Array<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> FloatArray; + +// This creates a pattern in the frame of image2, from the pixel is image1, +// based on the initial guess represented by the two quads x1, y1, and x2, y2. +template<typename Warp> +void CreateBrutePattern(const double *x1, const double *y1, + const double *x2, const double *y2, + const FloatImage &image1, + const FloatImage *image1_mask, + FloatArray *pattern, + FloatArray *mask, + int *origin_x, + int *origin_y) { + // Get integer bounding box of quad2 in image2. + int min_x = static_cast<int>(floor(*std::min_element(x2, x2 + 4))); + int min_y = static_cast<int>(floor(*std::min_element(y2, y2 + 4))); + int max_x = static_cast<int>(ceil (*std::max_element(x2, x2 + 4))); + int max_y = static_cast<int>(ceil (*std::max_element(y2, y2 + 4))); + + int w = max_x - min_x; + int h = max_y - min_y; + + pattern->resize(h, w); + mask->resize(h, w); + + Warp inverse_warp(x2, y2, x1, y1); + + // r,c are in the coordinate frame of image2. + for (int r = min_y; r < max_y; ++r) { + for (int c = min_x; c < max_x; ++c) { + // i and j are in the coordinate frame of the pattern in image2. + int i = r - min_y; + int j = c - min_x; + + double dst_x = c; + double dst_y = r; + double src_x; + double src_y; + inverse_warp.Forward(inverse_warp.parameters, + dst_x, dst_y, + &src_x, &src_y); + + if (PointInQuad(x1, y1, src_x, src_y)) { + (*pattern)(i, j) = SampleLinear(image1, src_y, src_x); + (*mask)(i, j) = 1.0; + if (image1_mask) { + (*mask)(i, j) = SampleLinear(*image1_mask, src_y, src_x);; + } + } else { + (*pattern)(i, j) = 0.0; + (*mask)(i, j) = 0.0; + } + } + } + *origin_x = min_x; + *origin_y = min_y; +} + +// Compute a translation-only estimate of the warp, using brute force search. A +// smarter implementation would use the FFT to compute the normalized cross +// correlation. Instead, this is a dumb implementation. Surprisingly, it is +// fast enough in practice. +// +// TODO(keir): The normalization is less effective for the brute force search +// than it is with the Ceres solver. It's unclear if this is a bug or due to +// the original frame being too different from the reprojected reference in the +// destination frame. +// +// The likely solution is to use the previous frame, instead of the original +// pattern, when doing brute initialization. Unfortunately that implies a +// totally different warping interface, since access to more than a the source +// and current destination frame is necessary. +template<typename Warp> +void BruteTranslationOnlyInitialize(const FloatImage &image1, + const FloatImage *image1_mask, + const FloatImage &image2, + const int num_extra_points, + const bool use_normalized_intensities, + const double *x1, const double *y1, + double *x2, double *y2) { + // Create the pattern to match in the space of image2, assuming our inital + // guess isn't too far from the template in image1. If there is no image1 + // mask, then the resulting mask is binary. + FloatArray pattern; + FloatArray mask; + int origin_x = -1, origin_y = -1; + CreateBrutePattern<Warp>(x1, y1, x2, y2, + image1, image1_mask, + &pattern, &mask, + &origin_x, &origin_y); + + // For normalization, premultiply the pattern by the inverse pattern mean. + double mask_sum = 1.0; + if (use_normalized_intensities) { + mask_sum = mask.sum(); + double inverse_pattern_mean = mask_sum / ((mask * pattern).sum()); + pattern *= inverse_pattern_mean; + } + + // Use Eigen on the images via maps for strong vectorization. + Map<const FloatArray> search(image2.Data(), image2.Height(), image2.Width()); + + // Try all possible locations inside the search area. Yes, everywhere. + // + // TODO(keir): There are a number of possible optimizations here. One choice + // is to make a grid and only try one out of every N possible samples. + // + // Another, slightly more clever idea, is to compute some sort of spatial + // frequency distribution of the pattern patch. If the spatial resolution is + // high (e.g. a grating pattern or fine lines) then checking every possible + // translation is necessary, since a 1-pixel shift may induce a massive + // change in the cost function. If the image is a blob or splotch with blurry + // edges, then fewer samples are necessary since a few pixels offset won't + // change the cost function much. + double best_sad = std::numeric_limits<double>::max(); + int best_r = -1; + int best_c = -1; + int w = pattern.cols(); + int h = pattern.rows(); + for (int r = 0; r < (image2.Height() - h); ++r) { + for (int c = 0; c < (image2.Width() - w); ++c) { + // Compute the weighted sum of absolute differences, Eigen style. Note + // that the block from the search image is never stored in a variable, to + // avoid copying overhead and permit inlining. + double sad; + if (use_normalized_intensities) { + // TODO(keir): It's really dumb to recompute the search mean for every + // shift. A smarter implementation would use summed area tables + // instead, reducing the mean calculation to an O(1) operation. + double inverse_search_mean = + mask_sum / ((mask * search.block(r, c, h, w)).sum()); + sad = (mask * (pattern - (search.block(r, c, h, w) * + inverse_search_mean))).abs().sum(); + } else { + sad = (mask * (pattern - search.block(r, c, h, w))).abs().sum(); + } + if (sad < best_sad) { + best_r = r; + best_c = c; + best_sad = sad; + } + } + } + CHECK_NE(best_r, -1); + CHECK_NE(best_c, -1); + + LG << "Brute force translation found a shift. " + << "best_c: " << best_c << ", best_r: " << best_r << ", " + << "origin_x: " << origin_x << ", origin_y: " << origin_y << ", " + << "dc: " << (best_c - origin_x) << ", " + << "dr: " << (best_r - origin_y) + << ", tried " << ((image2.Height() - h) * (image2.Width() - w)) + << " shifts."; + + // Apply the shift. + for (int i = 0; i < 4 + num_extra_points; ++i) { + x2[i] += best_c - origin_x; + y2[i] += best_r - origin_y; + } +} + +} // namespace + +template<typename Warp> +void TemplatedTrackRegion(const FloatImage &image1, + const FloatImage &image2, + const double *x1, const double *y1, + const TrackRegionOptions &options, + double *x2, double *y2, + TrackRegionResult *result) { + for (int i = 0; i < 4; ++i) { + LG << "P" << i << ": (" << x1[i] << ", " << y1[i] << "); guess (" + << x2[i] << ", " << y2[i] << "); (dx, dy): (" << (x2[i] - x1[i]) << ", " + << (y2[i] - y1[i]) << ")."; + } + if (options.use_normalized_intensities) { + LG << "Using normalized intensities."; + } + + // Bail early if the points are already outside. + if (!AllInBounds(image1, x1, y1)) { + result->termination = TrackRegionResult::SOURCE_OUT_OF_BOUNDS; + return; + } + if (!AllInBounds(image2, x2, y2)) { + result->termination = TrackRegionResult::DESTINATION_OUT_OF_BOUNDS; + return; + } + // TODO(keir): Check quads to ensure there is some area. + + // Keep a copy of the "original" guess for regularization. + double x2_original[4]; + double y2_original[4]; + for (int i = 0; i < 4; ++i) { + x2_original[i] = x2[i]; + y2_original[i] = y2[i]; + } + + // Prepare the image and gradient. + Array3Df image_and_gradient1; + Array3Df image_and_gradient2; + BlurredImageAndDerivativesChannels(image1, options.sigma, + &image_and_gradient1); + BlurredImageAndDerivativesChannels(image2, options.sigma, + &image_and_gradient2); + + // Possibly do a brute-force translation-only initialization. + if (SearchAreaTooBigForDescent(image2, x2, y2) && + options.use_brute_initialization) { + LG << "Running brute initialization..."; + BruteTranslationOnlyInitialize<Warp>(image_and_gradient1, + options.image1_mask, + image2, + options.num_extra_points, + options.use_normalized_intensities, + x1, y1, x2, y2); + for (int i = 0; i < 4; ++i) { + LG << "P" << i << ": (" << x1[i] << ", " << y1[i] << "); brute (" + << x2[i] << ", " << y2[i] << "); (dx, dy): (" << (x2[i] - x1[i]) + << ", " << (y2[i] - y1[i]) << ")."; + } + } + + // Prepare the initial warp parameters from the four correspondences. + // Note: This must happen after the brute initialization runs, since the + // brute initialization mutates x2 and y2 in place. + Warp warp(x1, y1, x2, y2); + + // Decide how many samples to use in the x and y dimensions. + int num_samples_x; + int num_samples_y; + PickSampling(x1, y1, x2, y2, &num_samples_x, &num_samples_y); + + + // Compute the warp from rectangular coordinates. + Mat3 canonical_homography = ComputeCanonicalHomography(x1, y1, + num_samples_x, + num_samples_y); + + ceres::Problem problem; + + // Construct the warp cost function. AutoDiffCostFunction takes ownership. + PixelDifferenceCostFunctor<Warp> *pixel_difference_cost_function = + new PixelDifferenceCostFunctor<Warp>(options, + image_and_gradient1, + image_and_gradient2, + canonical_homography, + num_samples_x, + num_samples_y, + warp); + problem.AddResidualBlock( + new ceres::AutoDiffCostFunction< + PixelDifferenceCostFunctor<Warp>, + ceres::DYNAMIC, + Warp::NUM_PARAMETERS>(pixel_difference_cost_function, + num_samples_x * num_samples_y), + NULL, + warp.parameters); + + // Construct the regularizing cost function + if (options.regularization_coefficient != 0.0) { + WarpRegularizingCostFunctor<Warp> *regularizing_warp_cost_function = + new WarpRegularizingCostFunctor<Warp>(options, + x1, y2, + x2_original, + y2_original, + warp); + + problem.AddResidualBlock( + new ceres::AutoDiffCostFunction< + WarpRegularizingCostFunctor<Warp>, + 8 /* num_residuals */, + Warp::NUM_PARAMETERS>(regularizing_warp_cost_function), + NULL, + warp.parameters); + } + + // Configure the solve. + ceres::Solver::Options solver_options; + solver_options.linear_solver_type = ceres::DENSE_QR; + solver_options.max_num_iterations = options.max_iterations; + solver_options.update_state_every_iteration = true; + solver_options.parameter_tolerance = 1e-16; + solver_options.function_tolerance = 1e-16; + + // Prevent the corners from going outside the destination image. + BoundaryCheckingCallback<Warp> callback(image2, warp, x1, y1); + solver_options.callbacks.push_back(&callback); + + // Run the solve. + ceres::Solver::Summary summary; + ceres::Solve(solver_options, &problem, &summary); + + LG << "Summary:\n" << summary.FullReport(); + + // Update the four points with the found solution; if the solver failed, then + // the warp parameters are the identity (so ignore failure). + // + // Also warp any extra points on the end of the array. + for (int i = 0; i < 4 + options.num_extra_points; ++i) { + warp.Forward(warp.parameters, x1[i], y1[i], x2 + i, y2 + i); + LG << "Warped point " << i << ": (" << x1[i] << ", " << y1[i] << ") -> (" + << x2[i] << ", " << y2[i] << "); (dx, dy): (" << (x2[i] - x1[i]) << ", " + << (y2[i] - y1[i]) << ")."; + } + + // TODO(keir): Update the result statistics. + // TODO(keir): Add a normalize-cross-correlation variant. + + CHECK_NE(summary.termination_type, ceres::USER_ABORT) << "Libmv bug."; + if (summary.termination_type == ceres::USER_ABORT) { + result->termination = TrackRegionResult::FELL_OUT_OF_BOUNDS; + return; + } +#define HANDLE_TERMINATION(termination_enum) \ + if (summary.termination_type == ceres::termination_enum) { \ + result->termination = TrackRegionResult::termination_enum; \ + return; \ + } + + // Avoid computing correlation for tracking failures. + HANDLE_TERMINATION(DID_NOT_RUN); + HANDLE_TERMINATION(NUMERICAL_FAILURE); + + // Otherwise, run a final correlation check. + if (options.minimum_correlation > 0.0) { + result->correlation = pixel_difference_cost_function-> + PearsonProductMomentCorrelationCoefficient(warp.parameters); + if (result->correlation < options.minimum_correlation) { + LG << "Failing with insufficient correlation."; + result->termination = TrackRegionResult::INSUFFICIENT_CORRELATION; + return; + } + } + + HANDLE_TERMINATION(PARAMETER_TOLERANCE); + HANDLE_TERMINATION(FUNCTION_TOLERANCE); + HANDLE_TERMINATION(GRADIENT_TOLERANCE); + HANDLE_TERMINATION(NO_CONVERGENCE); +#undef HANDLE_TERMINATION +}; + +void TrackRegion(const FloatImage &image1, + const FloatImage &image2, + const double *x1, const double *y1, + const TrackRegionOptions &options, + double *x2, double *y2, + TrackRegionResult *result) { + // Enum is necessary due to templated nature of autodiff. +#define HANDLE_MODE(mode_enum, mode_type) \ + if (options.mode == TrackRegionOptions::mode_enum) { \ + TemplatedTrackRegion<mode_type>(image1, image2, \ + x1, y1, \ + options, \ + x2, y2, \ + result); \ + return; \ + } + HANDLE_MODE(TRANSLATION, TranslationWarp); + HANDLE_MODE(TRANSLATION_SCALE, TranslationScaleWarp); + HANDLE_MODE(TRANSLATION_ROTATION, TranslationRotationWarp); + HANDLE_MODE(TRANSLATION_ROTATION_SCALE, TranslationRotationScaleWarp); + HANDLE_MODE(AFFINE, AffineWarp); + HANDLE_MODE(HOMOGRAPHY, HomographyWarp); +#undef HANDLE_MODE +} + +bool SamplePlanarPatch(const FloatImage &image, + const double *xs, const double *ys, + int num_samples_x, int num_samples_y, + FloatImage *patch, + double *warped_position_x, double *warped_position_y) { + // Bail early if the points are outside the image. + if (!AllInBounds(image, xs, ys)) { + LG << "Can't sample patch: out of bounds."; + return false; + } + + // Make the patch have the appropriate size, and match the depth of image. + patch->Resize(num_samples_y, num_samples_x, image.Depth()); + + // Compute the warp from rectangular coordinates. + Mat3 canonical_homography = ComputeCanonicalHomography(xs, ys, + num_samples_x, + num_samples_y); + + // Walk over the coordinates in the canonical space, sampling from the image + // in the original space and copying the result into the patch. + for (int r = 0; r < num_samples_y; ++r) { + for (int c = 0; c < num_samples_x; ++c) { + Vec3 image_position = canonical_homography * Vec3(c, r, 1); + image_position /= image_position(2); + SampleLinear(image, image_position(1), + image_position(0), + &(*patch)(r, c, 0)); + } + } + + Vec3 warped_position = canonical_homography.inverse() * Vec3(xs[4], ys[4], 1); + warped_position /= warped_position(2); + + *warped_position_x = warped_position(0); + *warped_position_y = warped_position(1); + + return true; +} + +} // namespace libmv diff --git a/extern/libmv/libmv/tracking/track_region.h b/extern/libmv/libmv/tracking/track_region.h new file mode 100644 index 00000000000..0de11239da6 --- /dev/null +++ b/extern/libmv/libmv/tracking/track_region.h @@ -0,0 +1,146 @@ +// Copyright (c) 2012 libmv authors. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to +// deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. + +#ifndef LIBMV_TRACKING_TRACK_REGION_H_ + +// Necessary for M_E when building with MSVC. +#define _USE_MATH_DEFINES + +#include "libmv/tracking/esm_region_tracker.h" + +#include "libmv/image/image.h" +#include "libmv/image/sample.h" +#include "libmv/numeric/numeric.h" + +namespace libmv { + +struct TrackRegionOptions { + TrackRegionOptions(); + + enum Mode { + TRANSLATION, + TRANSLATION_ROTATION, + TRANSLATION_SCALE, + TRANSLATION_ROTATION_SCALE, + AFFINE, + HOMOGRAPHY, + }; + Mode mode; + + double minimum_correlation; + int max_iterations; + + // Use the "Efficient Second-order Minimization" scheme. This increases + // convergence speed at the cost of more per-iteration work. + bool use_esm; + + // If true, apply a brute-force translation-only search before attempting the + // full search. This is not enabled if the destination image ("image2") is + // too small; in that case eithen the basin of attraction is close enough + // that the nearby minima is correct, or the search area is too small. + bool use_brute_initialization; + + // If true, normalize the image patches by their mean before doing the sum of + // squared error calculation. This is reasonable since the effect of + // increasing light intensity is multiplicative on the pixel intensities. + // + // Note: This does nearly double the solving time, so it is not advised to + // turn this on all the time. + bool use_normalized_intensities; + + // The size in pixels of the blur kernel used to both smooth the image and + // take the image derivative. + double sigma; + + // Extra points that should get transformed by the warp. This is useful + // because the actual warp parameters are not exposed. + int num_extra_points; + + // For motion models other than translation, the optimizer sometimes has + // trouble deciding what to do around flat areas in the cost function. This + // leads to the optimizer picking poor solutions near the minimum. Visually, + // the effect is that the quad corners jiggle around, even though the center + // of the patch is well estimated. regularization_coefficient controls a term + // in the sum of squared error cost that makes it expensive for the optimizer + // to pick a warp that changes the shape of the patch dramatically (e.g. + // rotating, scaling, skewing, etc). + // + // In particular it adds an 8-residual cost function to the optimization, + // where each corner induces 2 residuals: the difference between the warped + // and the initial guess. However, the patch centroids are subtracted so that + // only patch distortions are penalized. + // + // If zero, no regularization is used. + double regularization_coefficient; + + // If non-null, this is used as the pattern mask. It should match the size of + // image1, even though only values inside the image1 quad are examined. The + // values must be in the range 0.0 to 0.1. + FloatImage *image1_mask; +}; + +struct TrackRegionResult { + enum Termination { + // Ceres termination types, duplicated; though, not the int values. + PARAMETER_TOLERANCE, + FUNCTION_TOLERANCE, + GRADIENT_TOLERANCE, + NO_CONVERGENCE, + DID_NOT_RUN, + NUMERICAL_FAILURE, + + // Libmv specific errors. + SOURCE_OUT_OF_BOUNDS, + DESTINATION_OUT_OF_BOUNDS, + FELL_OUT_OF_BOUNDS, + INSUFFICIENT_CORRELATION, + CONFIGURATION_ERROR, + }; + Termination termination; + + int num_iterations; + double correlation; + + // Final parameters? + bool used_brute_translation_initialization; +}; + +// Always needs 4 correspondences. +void TrackRegion(const FloatImage &image1, + const FloatImage &image2, + const double *x1, const double *y1, + const TrackRegionOptions &options, + double *x2, double *y2, + TrackRegionResult *result); + +// Sample a "canonical" version of the passed planar patch, using bilinear +// sampling. The passed corners must be within the image, and have at least two +// pixels of border around them. (so e.g. a corner of the patch cannot lie +// directly on the edge of the image). Four corners are always required. All +// channels are interpolated. +bool SamplePlanarPatch(const FloatImage &image, + const double *xs, const double *ys, + int num_samples_x, int num_samples_y, + FloatImage *patch, + double *warped_position_x, double *warped_position_y); + +} // namespace libmv + +#endif // LIBMV_TRACKING_TRACK_REGION_H_ diff --git a/extern/libmv/mkfiles.sh b/extern/libmv/mkfiles.sh index fe84c357de4..c7c8c33f725 100755 --- a/extern/libmv/mkfiles.sh +++ b/extern/libmv/mkfiles.sh @@ -1,4 +1,4 @@ #!/bin/sh find ./libmv/ -type f | sed -r 's/^\.\///' | sort > files.txt -find ./third_party/ -type f | sed -r 's/^\.\///' | sort >> files.txt +find ./third_party/ -mindepth 2 -type f | grep -v third_party/ceres | sed -r 's/^\.\///' | sort >> files.txt diff --git a/extern/libmv/third_party/CMakeLists.txt b/extern/libmv/third_party/CMakeLists.txt new file mode 100644 index 00000000000..6212fe480b1 --- /dev/null +++ b/extern/libmv/third_party/CMakeLists.txt @@ -0,0 +1,2 @@ + +add_subdirectory(ceres) diff --git a/extern/libmv/third_party/SConscript b/extern/libmv/third_party/SConscript new file mode 100644 index 00000000000..b05692e385f --- /dev/null +++ b/extern/libmv/third_party/SConscript @@ -0,0 +1,3 @@ +#!/usr/bin/python + +SConscript(['ceres/SConscript']) diff --git a/extern/libmv/third_party/ceres/CMakeLists.txt b/extern/libmv/third_party/ceres/CMakeLists.txt new file mode 100644 index 00000000000..5207bddec12 --- /dev/null +++ b/extern/libmv/third_party/ceres/CMakeLists.txt @@ -0,0 +1,218 @@ +# ***** BEGIN GPL LICENSE BLOCK ***** +# +# This program is free software; you can redistribute it and/or +# modify it under the terms of the GNU General Public License +# as published by the Free Software Foundation; either version 2 +# of the License, or (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program; if not, write to the Free Software Foundation, +# Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. +# +# The Original Code is Copyright (C) 2012, Blender Foundation +# All rights reserved. +# +# Contributor(s): Blender Foundation, +# Sergey Sharybin +# +# ***** END GPL LICENSE BLOCK ***** + +# NOTE: This file is automatically generated by bundle.sh script +# If you're doing changes in this file, please update template +# in that script too + +set(INC + . + ../../../Eigen3 + include + internal + ../gflags +) + +set(INC_SYS +) + +set(SRC + internal/ceres/block_evaluate_preparer.cc + internal/ceres/block_jacobian_writer.cc + internal/ceres/block_jacobi_preconditioner.cc + internal/ceres/block_random_access_dense_matrix.cc + internal/ceres/block_random_access_matrix.cc + internal/ceres/block_random_access_sparse_matrix.cc + internal/ceres/block_sparse_matrix.cc + internal/ceres/block_structure.cc + internal/ceres/canonical_views_clustering.cc + internal/ceres/cgnr_solver.cc + internal/ceres/compressed_row_jacobian_writer.cc + internal/ceres/compressed_row_sparse_matrix.cc + internal/ceres/conditioned_cost_function.cc + internal/ceres/conjugate_gradients_solver.cc + internal/ceres/corrector.cc + internal/ceres/dense_qr_solver.cc + internal/ceres/dense_sparse_matrix.cc + internal/ceres/detect_structure.cc + internal/ceres/evaluator.cc + internal/ceres/file.cc + internal/ceres/generated/schur_eliminator_2_2_2.cc + internal/ceres/generated/schur_eliminator_2_2_3.cc + internal/ceres/generated/schur_eliminator_2_2_4.cc + internal/ceres/generated/schur_eliminator_2_2_d.cc + internal/ceres/generated/schur_eliminator_2_3_3.cc + internal/ceres/generated/schur_eliminator_2_3_4.cc + internal/ceres/generated/schur_eliminator_2_3_9.cc + internal/ceres/generated/schur_eliminator_2_3_d.cc + internal/ceres/generated/schur_eliminator_2_4_3.cc + internal/ceres/generated/schur_eliminator_2_4_4.cc + internal/ceres/generated/schur_eliminator_2_4_d.cc + internal/ceres/generated/schur_eliminator_4_4_2.cc + internal/ceres/generated/schur_eliminator_4_4_3.cc + internal/ceres/generated/schur_eliminator_4_4_4.cc + internal/ceres/generated/schur_eliminator_4_4_d.cc + internal/ceres/generated/schur_eliminator_d_d_d.cc + internal/ceres/gradient_checking_cost_function.cc + internal/ceres/implicit_schur_complement.cc + internal/ceres/iterative_schur_complement_solver.cc + internal/ceres/levenberg_marquardt.cc + internal/ceres/linear_least_squares_problems.cc + internal/ceres/linear_operator.cc + internal/ceres/linear_solver.cc + internal/ceres/local_parameterization.cc + internal/ceres/loss_function.cc + internal/ceres/normal_prior.cc + internal/ceres/partitioned_matrix_view.cc + internal/ceres/problem.cc + internal/ceres/problem_impl.cc + internal/ceres/program.cc + internal/ceres/residual_block.cc + internal/ceres/residual_block_utils.cc + internal/ceres/runtime_numeric_diff_cost_function.cc + internal/ceres/schur_complement_solver.cc + internal/ceres/schur_eliminator.cc + internal/ceres/schur_ordering.cc + internal/ceres/scratch_evaluate_preparer.cc + internal/ceres/solver.cc + internal/ceres/solver_impl.cc + internal/ceres/sparse_matrix.cc + internal/ceres/sparse_normal_cholesky_solver.cc + internal/ceres/split.cc + internal/ceres/stringprintf.cc + internal/ceres/suitesparse.cc + internal/ceres/triplet_sparse_matrix.cc + internal/ceres/types.cc + internal/ceres/visibility_based_preconditioner.cc + internal/ceres/visibility.cc + + include/ceres/autodiff_cost_function.h + include/ceres/ceres.h + include/ceres/conditioned_cost_function.h + include/ceres/cost_function.h + include/ceres/internal/autodiff.h + include/ceres/internal/eigen.h + include/ceres/internal/fixed_array.h + include/ceres/internal/macros.h + include/ceres/internal/manual_constructor.h + include/ceres/internal/port.h + include/ceres/internal/scoped_ptr.h + include/ceres/iteration_callback.h + include/ceres/jet.h + include/ceres/local_parameterization.h + include/ceres/loss_function.h + include/ceres/normal_prior.h + include/ceres/numeric_diff_cost_function.h + include/ceres/problem.h + include/ceres/rotation.h + include/ceres/sized_cost_function.h + include/ceres/solver.h + include/ceres/types.h + internal/ceres/block_evaluate_preparer.h + internal/ceres/block_jacobian_writer.h + internal/ceres/block_jacobi_preconditioner.h + internal/ceres/block_random_access_dense_matrix.h + internal/ceres/block_random_access_matrix.h + internal/ceres/block_random_access_sparse_matrix.h + internal/ceres/block_sparse_matrix.h + internal/ceres/block_structure.h + internal/ceres/canonical_views_clustering.h + internal/ceres/casts.h + internal/ceres/cgnr_linear_operator.h + internal/ceres/cgnr_solver.h + internal/ceres/collections_port.h + internal/ceres/compressed_row_jacobian_writer.h + internal/ceres/compressed_row_sparse_matrix.h + internal/ceres/conjugate_gradients_solver.h + internal/ceres/corrector.h + internal/ceres/dense_jacobian_writer.h + internal/ceres/dense_qr_solver.h + internal/ceres/dense_sparse_matrix.h + internal/ceres/detect_structure.h + internal/ceres/evaluator.h + internal/ceres/file.h + internal/ceres/gradient_checking_cost_function.h + internal/ceres/graph_algorithms.h + internal/ceres/graph.h + internal/ceres/implicit_schur_complement.h + internal/ceres/integral_types.h + internal/ceres/iterative_schur_complement_solver.h + internal/ceres/levenberg_marquardt.h + internal/ceres/linear_least_squares_problems.h + internal/ceres/linear_operator.h + internal/ceres/linear_solver.h + internal/ceres/map_util.h + internal/ceres/matrix_proto.h + internal/ceres/minimizer.h + internal/ceres/mutex.h + internal/ceres/parameter_block.h + internal/ceres/partitioned_matrix_view.h + internal/ceres/problem_impl.h + internal/ceres/program_evaluator.h + internal/ceres/program.h + internal/ceres/random.h + internal/ceres/residual_block.h + internal/ceres/residual_block_utils.h + internal/ceres/runtime_numeric_diff_cost_function.h + internal/ceres/schur_complement_solver.h + internal/ceres/schur_eliminator.h + internal/ceres/schur_eliminator_impl.h + internal/ceres/schur_ordering.h + internal/ceres/scratch_evaluate_preparer.h + internal/ceres/solver_impl.h + internal/ceres/sparse_matrix.h + internal/ceres/sparse_normal_cholesky_solver.h + internal/ceres/stl_util.h + internal/ceres/stringprintf.h + internal/ceres/suitesparse.h + internal/ceres/triplet_sparse_matrix.h + internal/ceres/visibility_based_preconditioner.h + internal/ceres/visibility.h +) + +if(WIN32) + list(APPEND INC + ../glog/src/windows + ) + + if(NOT MINGW) + list(APPEND INC + third_party/msinttypes + ) + endif() +else() + list(APPEND INC + ../glog/src + ) +endif() + +add_definitions( + -DCERES_HAVE_PTHREAD + -D"CERES_HASH_NAMESPACE_START=namespace std { namespace tr1 {" + -D"CERES_HASH_NAMESPACE_END=}}" + -DCERES_NO_SUITESPARSE + -DCERES_DONT_HAVE_PROTOCOL_BUFFERS +) + +blender_add_lib(extern_ceres "${SRC}" "${INC}" "${INC_SYS}") diff --git a/extern/libmv/third_party/ceres/ChangeLog b/extern/libmv/third_party/ceres/ChangeLog new file mode 100644 index 00000000000..6e919658f13 --- /dev/null +++ b/extern/libmv/third_party/ceres/ChangeLog @@ -0,0 +1,324 @@ +commit ca72152362ae1f4b9928c012e74b4d49d094a4ca +Merge: d297f8d 0a04199 +Author: Keir Mierle <mierle@gmail.com> +Date: Wed May 9 13:10:59 2012 -0700 + + Merge branch 'master' into windows + +commit 0a04199ef279cc9ea97f665fed8e7fae717813c3 +Merge: fdeb577 f2571f1 +Author: Keir Mierle <mierle@gmail.com> +Date: Wed May 9 12:54:56 2012 -0700 + + Merge branch 'master' of https://code.google.com/p/ceres-solver + +commit fdeb5772cc5eeebca4d776d220d80cc91b6d0f74 +Author: Keir Mierle <mierle@gmail.com> +Date: Wed May 9 07:38:07 2012 -0700 + + Support varying numbers of residuals in autodiff. + + This commit modifies the only function in autodiff that takes a + templated number of outputs (i.e. residuals) and makes that + template parameter a normal parameter. With that change, it + is a trivial matter to support a dynamic number of residuals. + + The API for dynamic residuals is to pass a fake number of + residuals as the second template argument to + AutoDiffCostFunction, and to pass the real number of + parameters as a second constructor argument. + +commit da3e0563cc12e08e7b3e0fbf11d9cc8cfe9658aa +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Wed May 9 11:57:47 2012 -0700 + + Typo corrections in the documentation from Bing + +commit aa9526d8e8fb34c23d63e3af5bf9239b0c4ea603 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Tue May 8 21:22:09 2012 -0700 + + Share search paths across various library searches. + Fix typos in glog search. + Split the error messages for include and lib. + Enable building of tests by default. + Made building on homebrew installations a bit better. + Remove temporary variables for glog and gflags. + +commit f2571f186850ed3dd316236ac4be488979df7d30 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Wed May 9 11:57:47 2012 -0700 + + Typo corrections in the documentation from Bing + +commit 8f7f11ff7d07737435428a2620c52419cf99f98e +Merge: e6c17c4 eaccbb3 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Wed May 9 11:34:15 2012 -0700 + + Merge branch 'master' of https://code.google.com/p/ceres-solver + +commit e6c17c4c9d9307218f6f739cea39bc2d87733d4d +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Tue May 8 21:22:09 2012 -0700 + + Share search paths across various library searches. + Fix typos in glog search. + Split the error messages for include and lib. + Enable building of tests by default. + Made building on homebrew installations a bit better. + Remove temporary variables for glog and gflags. + +commit eaccbb345614c0d24c5e21fa931f470cfda874df +Author: Keir Mierle <mierle@gmail.com> +Date: Wed May 9 05:31:29 2012 -0700 + + Remove unused template parameter from VariadicEvaluate. + +commit 82f4b88c34b0b2cf85064e5fc20e374e978b2e3b +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Sun May 6 21:05:28 2012 -0700 + + Extend support writing linear least squares problems to disk. + + 1. Make the mechanism for writing problems to disk, generic and + controllable using an enum DumpType visible in the API. + + 2. Instead of single file containing protocol buffers, now matrices can + be written in a matlab/octave friendly format. This is now the default. + + 3. The support for writing problems to disk is moved into + linear_least_squares_problem.cc/h + + 4. SparseMatrix now has a ToTextFile virtual method which is + implemented by each of its subclasses to write a (i,j,s) triplets. + + 5. Minor changes to simple_bundle_adjuster to enable logging at startup. + +commit d297f8d3d3f5025c24752f0f4c1ec2469a769f99 +Merge: 7e74d81 f8bd7fa +Author: Keir Mierle <mierle@gmail.com> +Date: Tue May 8 05:39:56 2012 -0700 + + Merge branch 'master' into windows + +commit f8bd7fa9aa9dbf64b6165606630287cf8cf21194 +Author: Keir Mierle <mierle@gmail.com> +Date: Tue May 8 05:39:32 2012 -0700 + + Small tweaks to the block jacobi preconditioner. + +commit 7e74d81ad57a159f14110eb5348b3bc7990b8bd4 +Merge: ecd7c8d e2a6cdc +Author: Keir Mierle <mierle@gmail.com> +Date: Mon May 7 07:02:49 2012 -0700 + + Merge branch 'master' into windows + +commit e2a6cdc0816af9d0c77933f5017f137da3d52a35 +Author: Keir Mierle <mierle@gmail.com> +Date: Mon May 7 06:39:56 2012 -0700 + + Address some of the comments on CGNR patch + + - Rename BlockDiagonalPreconditioner to BlockJacobiPreconditioner + - Include the diagonal in the block jacobi preconditioner. + - Better flag help for eta. + - Enable test for CGNR + - Rename CONJUGATE_GRADIENTS to CGNR. + - etc. + +commit 1b95dc580aa5d89be021c0915e26df83f18013bb +Merge: 211812a 7646039 +Author: Keir Mierle <mierle@gmail.com> +Date: Mon May 7 04:34:10 2012 -0700 + + Merge branch 'master' of https://code.google.com/p/ceres-solver + +commit 211812a57360d2011cbcfd115cd55e0eb73600db +Author: Keir Mierle <mierle@gmail.com> +Date: Mon May 7 04:33:50 2012 -0700 + + Better error handling in bundle_adjuster.cc + +commit 7646039ad9672b267495f5b31925473ad3022ac8 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Sun May 6 22:02:19 2012 -0700 + + Kashif's corrections to the docs + +commit 0d2d34148d10c5c7e924b3ca82ad2b237573ef64 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Sun May 6 21:16:03 2012 -0700 + + glog minimum version requirements + + Building Ceres requires version 0.3.1 or better of glog. + Fedora 16 ships with a busted version 0.3. + + issue 15 contains the gory details. + + Added a note to the build documentation to this effect. + +commit 39efc5ec4b64b8f5a2c5a3dbacdbc45421221547 +Author: Keir Mierle <mierle@gmail.com> +Date: Sun May 6 16:09:52 2012 -0700 + + Fix tests broken by the CGNR change. + +commit 3faa08b7f7c4ac73661c6a15a6824c12080dfcb1 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Sun May 6 16:08:22 2012 -0700 + + Formatting fixed based on Keir's comments and extended the tests + +commit 4f21c68409bc478c431a9b6aedf9e5cfdf11d2f3 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Sun May 6 15:33:47 2012 -0700 + + Fix the struct weak ordering used by independent set ordering, tests for it + +commit 887b156b917ccd4c172484452b059d33ea45f4f0 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Sun May 6 15:14:47 2012 -0700 + + fix he degree ordering routine + +commit ecd7c8df2af19404dc394b36bbe96e9db3bce840 +Author: Keir Mierle <mierle@gmail.com> +Date: Sun May 6 00:09:41 2012 -0700 + + First step towards windows compatibilty + + This adds some small changes to Ceres to make it mostly + compile on Windows. There are still issues with the + hash map use in schur_ordering.cc but I will fix those + shortly. + +commit f7898fba1b92f0e996571b5bfa22a37f5e3644de +Author: Keir Mierle <mierle@gmail.com> +Date: Sat May 5 20:55:08 2012 -0700 + + Add a general sparse iterative solver: CGNR + + This adds a new LinearOperator which implements symmetric + products of a matrix, and a new CGNR solver to leverage + CG to directly solve the normal equations. This also + includes a block diagonal preconditioner. In experiments + on problem-16, the non-preconditioned version is about + 1/5 the speed of SPARSE_SCHUR, and the preconditioned + version using block cholesky is about 20% slower than + SPARSE_SCHUR. + +commit 0a359d6198d257776a8831c3eb98f64ee91cf836 +Author: Keir Mierle <mierle@gmail.com> +Date: Sat May 5 20:33:46 2012 -0700 + + Comment formatting. + +commit db4ec9312bb2f1ca7b2337812f6bad6cdd75b227 +Author: Keir Mierle <mierle@gmail.com> +Date: Sat May 5 20:33:16 2012 -0700 + + Comment formatting + +commit f10163aaf3e57f52551bcd60bbdae873890a49dd +Author: Keir Mierle <mierle@gmail.com> +Date: Fri May 4 21:33:53 2012 -0700 + + Warn about disabled schur specializations. + + This commit brought to you from 30,000ft. + +commit ad7b2b4aaf3ccc51f2b854febd53a9df54686cfe +Author: Keir Mierle <mierle@gmail.com> +Date: Fri May 4 20:15:28 2012 -0700 + + Add vim swapfiles to .gitignore + +commit 6447219826bf6e47b0c99d9ff0eaf5e2ba573d79 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Thu May 3 21:53:07 2012 -0700 + + 1. Changes the tutorial to refer to BriefReport. + 2. Some of the enums have commas at the end. + 3. Fix a bug in the default value of circle_fit.cc in the examples. + +commit 30c5f93c7f88dec49f76168663372772e06f17f5 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Thu May 3 10:44:43 2012 -0700 + + Rework the glog and gtest path checking to be consistent with the rest of the file and disable the dashboard support enabled by the earlier ctesting related patch. + +commit f10b033eb4aca77919987bc551d16d8a88b10110 +Merge: cc38774 e0a52a9 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Thu May 3 08:45:20 2012 -0700 + + Merge branch 'ctest' + +commit e0a52a993394e73bc7f7db8d520728926feab83e +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Thu May 3 08:43:34 2012 -0700 + + Arnaus Gelas' patch to add better path searching for gflags and glog + +commit a9b8e815e1c026599734510399b10f4cf014c9cd +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Thu May 3 08:41:52 2012 -0700 + + Arnaus Gelas' patch to add .gitignore + +commit a0cefc3347c32b2065053bbaff4f34d11529d931 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Thu May 3 08:38:33 2012 -0700 + + Arnaus Gelas' patch to move to Ctest + +commit cc38774d74e287704915282425fbd16818a72ec3 +Author: Keir Mierle <mierle@gmail.com> +Date: Thu May 3 01:27:50 2012 -0700 + + Clarify ProgramEvaluator comments. + +commit 017c9530df557863f78212fb5ccd02814baa9fa8 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Wed May 2 08:21:59 2012 -0700 + + Mac OS X build instructions are much simpler, as homebrew takes care of gflags when glog is brought in. Also CMAKE does not need any flags to do the default thing + +commit 92d5ab5f8ae6fe355c30b606a5f230415ee0494b +Author: Keir Mierle <mierle@gmail.com> +Date: Tue May 1 18:33:08 2012 -0700 + + Link BLAS explicitly on non-Mac platforms + + Fixes issue #3. + +commit df3e54eb4a6b001b7f0560a2da73a5bd7f18615e +Author: Keir Mierle <mierle@gmail.com> +Date: Tue May 1 18:22:51 2012 -0700 + + Fix link order of CHOLMOD + + This was working by accident due to dynamic linking. Fixes issue #2. + +commit f477a3835329e2b48eb20c34c631a480b0f0d5bf +Author: Keir Mierle <mierle@gmail.com> +Date: Tue May 1 18:10:48 2012 -0700 + + Fix Eigen search paths + + Fixes issue #1 on http://code.google.com/p/ceres-solver. + +commit 17fbc8ebb894c1d22bb3b0b02ea1394b580120f8 +Author: Sameer Agarwal <sameeragarwal@google.com> +Date: Tue May 1 00:21:19 2012 -0700 + + Minor changes to the documentation. Formatting, and typos. + +commit 8ebb0730388045570f22b89fe8672c860cd2ad1b +Author: Keir Mierle <mierle@gmail.com> +Date: Mon Apr 30 23:09:08 2012 -0700 + + Initial commit of Ceres Solver. diff --git a/extern/libmv/third_party/ceres/LICENSE b/extern/libmv/third_party/ceres/LICENSE new file mode 100644 index 00000000000..2e3ead5ed45 --- /dev/null +++ b/extern/libmv/third_party/ceres/LICENSE @@ -0,0 +1,27 @@ +Ceres Solver - A fast non-linear least squares minimizer +Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +http://code.google.com/p/ceres-solver/ + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, + this list of conditions and the following disclaimer. +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +* Neither the name of Google Inc. nor the names of its contributors may be + used to endorse or promote products derived from this software without + specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +POSSIBILITY OF SUCH DAMAGE. diff --git a/extern/libmv/third_party/ceres/README b/extern/libmv/third_party/ceres/README new file mode 100644 index 00000000000..8dd8ccf91a1 --- /dev/null +++ b/extern/libmv/third_party/ceres/README @@ -0,0 +1,3 @@ +Ceres Solver - A non-linear least squares minimizer +================================================== +Please see ceres.pdf in docs/ for a tutorial and reference. diff --git a/extern/libmv/third_party/ceres/SConscript b/extern/libmv/third_party/ceres/SConscript new file mode 100644 index 00000000000..d8b2b8520d7 --- /dev/null +++ b/extern/libmv/third_party/ceres/SConscript @@ -0,0 +1,34 @@ +#!/usr/bin/python + +# NOTE: This file is automatically generated by bundle.sh script +# If you're doing changes in this file, please update template +# in that script too + +import sys +import os + +Import('env') + +src = [] +defs = [] + +src += env.Glob('internal/ceres/*.cc') +src += env.Glob('internal/ceres/generated/*.cc') + +defs.append('CERES_HAVE_PTHREAD') +defs.append('CERES_HASH_NAMESPACE_START=namespace std { namespace tr1 {') +defs.append('CERES_HASH_NAMESPACE_END=}}') +defs.append('CERES_NO_SUITESPARSE') +defs.append('CERES_DONT_HAVE_PROTOCOL_BUFFERS') + +incs = '. ../../ ../../../Eigen3 ./include ./internal ../gflags' + +if env['OURPLATFORM'] in ('win32-vc', 'win32-mingw', 'linuxcross', 'win64-vc', 'win64-mingw'): + if env['OURPLATFORM'] in ('win32-vc', 'win64-vc'): + incs += ' ../msinttypes' + + incs += ' ../glog/src/windows' +else: + incs += ' ../glog/src' + +env.BlenderLib ( libname = 'extern_ceres', sources=src, includes=Split(incs), defines=defs, libtype=['extern', 'player'], priority=[20,137]) diff --git a/extern/libmv/third_party/ceres/bundle.sh b/extern/libmv/third_party/ceres/bundle.sh new file mode 100644 index 00000000000..f54342180db --- /dev/null +++ b/extern/libmv/third_party/ceres/bundle.sh @@ -0,0 +1,185 @@ +#!/bin/sh + +if [ "x$1" = "x--i-really-know-what-im-doing" ] ; then + echo Proceeding as requested by command line ... +else + echo "*** Please run again with --i-really-know-what-im-doing ..." + exit 1 +fi + +repo="https://code.google.com/p/ceres-solver/" +branch="windows" +tmp=`mktemp -d` + +GIT="git --git-dir $tmp/ceres/.git --work-tree $tmp/ceres" + +git clone $repo $tmp/ceres + +if [ $branch != "master" ]; then + $GIT checkout -t remotes/origin/$branch +fi + +$GIT log -n 50 > ChangeLog + +for p in `cat ./patches/series`; do + echo "Applying patch $p..." + cat ./patches/$p | patch -d $tmp/ceres -p1 +done + +find include -type f -not -iwholename '*.svn*' -exec rm -rf {} \; +find internal -type f -not -iwholename '*.svn*' -exec rm -rf {} \; + +cat "files.txt" | while read f; do + mkdir -p `dirname $f` + cp $tmp/ceres/$f $f +done + +rm -rf $tmp + +sources=`find ./include ./internal -type f -iname '*.cc' -or -iname '*.cpp' -or -iname '*.c' | sed -r 's/^\.\//\t/' | sort -d` +headers=`find ./include ./internal -type f -iname '*.h' | sed -r 's/^\.\//\t/' | sort -d` + +src_dir=`find ./internal -type f -iname '*.cc' -exec dirname {} \; -or -iname '*.cpp' -exec dirname {} \; -or -iname '*.c' -exec dirname {} \; | sed -r 's/^\.\//\t/' | sort -d | uniq` +src="" +for x in $src_dir $src_third_dir; do + t="" + + if test `echo "$x" | grep -c glog ` -eq 1; then + continue; + fi + + if stat $x/*.cpp > /dev/null 2>&1; then + t="src += env.Glob('`echo $x'/*.cpp'`')" + fi + + if stat $x/*.c > /dev/null 2>&1; then + if [ -z "$t" ]; then + t="src += env.Glob('`echo $x'/*.c'`')" + else + t="$t + env.Glob('`echo $x'/*.c'`')" + fi + fi + + if stat $x/*.cc > /dev/null 2>&1; then + if [ -z "$t" ]; then + t="src += env.Glob('`echo $x'/*.cc'`')" + else + t="$t + env.Glob('`echo $x'/*.cc'`')" + fi + fi + + if [ -z "$src" ]; then + src=$t + else + src=`echo "$src\n$t"` + fi +done + +cat > CMakeLists.txt << EOF +# ***** BEGIN GPL LICENSE BLOCK ***** +# +# This program is free software; you can redistribute it and/or +# modify it under the terms of the GNU General Public License +# as published by the Free Software Foundation; either version 2 +# of the License, or (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program; if not, write to the Free Software Foundation, +# Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. +# +# The Original Code is Copyright (C) 2012, Blender Foundation +# All rights reserved. +# +# Contributor(s): Blender Foundation, +# Sergey Sharybin +# +# ***** END GPL LICENSE BLOCK ***** + +# NOTE: This file is automatically generated by bundle.sh script +# If you're doing changes in this file, please update template +# in that script too + +set(INC + . + ../../../Eigen3 + include + internal + ../gflags +) + +set(INC_SYS +) + +set(SRC +${sources} + +${headers} +) + +if(WIN32) + list(APPEND INC + ../glog/src/windows + ) + + if(NOT MINGW) + list(APPEND INC + third_party/msinttypes + ) + endif() +else() + list(APPEND INC + ../glog/src + ) +endif() + +add_definitions( + -DCERES_HAVE_PTHREAD + -D"CERES_HASH_NAMESPACE_START=namespace std { namespace tr1 {" + -D"CERES_HASH_NAMESPACE_END=}}" + -DCERES_NO_SUITESPARSE + -DCERES_DONT_HAVE_PROTOCOL_BUFFERS +) + +blender_add_lib(extern_ceres "\${SRC}" "\${INC}" "\${INC_SYS}") +EOF + +cat > SConscript << EOF +#!/usr/bin/python + +# NOTE: This file is automatically generated by bundle.sh script +# If you're doing changes in this file, please update template +# in that script too + +import sys +import os + +Import('env') + +src = [] +defs = [] + +$src + +defs.append('CERES_HAVE_PTHREAD') +defs.append('CERES_HASH_NAMESPACE_START=namespace std { namespace tr1 {') +defs.append('CERES_HASH_NAMESPACE_END=}}') +defs.append('CERES_NO_SUITESPARSE') +defs.append('CERES_DONT_HAVE_PROTOCOL_BUFFERS') + +incs = '. ../../ ../../../Eigen3 ./include ./internal ../gflags' + +if env['OURPLATFORM'] in ('win32-vc', 'win32-mingw', 'linuxcross', 'win64-vc', 'win64-mingw'): + if env['OURPLATFORM'] in ('win32-vc', 'win64-vc'): + incs += ' ../msinttypes' + + incs += ' ../glog/src/windows' +else: + incs += ' ../glog/src' + +env.BlenderLib ( libname = 'extern_ceres', sources=src, includes=Split(incs), defines=defs, libtype=['extern', 'player'], priority=[20,137]) +EOF diff --git a/extern/libmv/third_party/ceres/files.txt b/extern/libmv/third_party/ceres/files.txt new file mode 100644 index 00000000000..e9d7f585260 --- /dev/null +++ b/extern/libmv/third_party/ceres/files.txt @@ -0,0 +1,150 @@ +include/ceres/autodiff_cost_function.h +include/ceres/ceres.h +include/ceres/conditioned_cost_function.h +include/ceres/cost_function.h +include/ceres/internal/autodiff.h +include/ceres/internal/eigen.h +include/ceres/internal/fixed_array.h +include/ceres/internal/macros.h +include/ceres/internal/manual_constructor.h +include/ceres/internal/port.h +include/ceres/internal/scoped_ptr.h +include/ceres/iteration_callback.h +include/ceres/jet.h +include/ceres/local_parameterization.h +include/ceres/loss_function.h +include/ceres/normal_prior.h +include/ceres/numeric_diff_cost_function.h +include/ceres/problem.h +include/ceres/rotation.h +include/ceres/sized_cost_function.h +include/ceres/solver.h +include/ceres/types.h +internal/ceres/block_evaluate_preparer.cc +internal/ceres/block_evaluate_preparer.h +internal/ceres/block_jacobian_writer.cc +internal/ceres/block_jacobian_writer.h +internal/ceres/block_jacobi_preconditioner.cc +internal/ceres/block_jacobi_preconditioner.h +internal/ceres/block_random_access_dense_matrix.cc +internal/ceres/block_random_access_dense_matrix.h +internal/ceres/block_random_access_matrix.cc +internal/ceres/block_random_access_matrix.h +internal/ceres/block_random_access_sparse_matrix.cc +internal/ceres/block_random_access_sparse_matrix.h +internal/ceres/block_sparse_matrix.cc +internal/ceres/block_sparse_matrix.h +internal/ceres/block_structure.cc +internal/ceres/block_structure.h +internal/ceres/canonical_views_clustering.cc +internal/ceres/canonical_views_clustering.h +internal/ceres/casts.h +internal/ceres/cgnr_linear_operator.h +internal/ceres/cgnr_solver.cc +internal/ceres/cgnr_solver.h +internal/ceres/collections_port.h +internal/ceres/compressed_row_jacobian_writer.cc +internal/ceres/compressed_row_jacobian_writer.h +internal/ceres/compressed_row_sparse_matrix.cc +internal/ceres/compressed_row_sparse_matrix.h +internal/ceres/conditioned_cost_function.cc +internal/ceres/conjugate_gradients_solver.cc +internal/ceres/conjugate_gradients_solver.h +internal/ceres/corrector.cc +internal/ceres/corrector.h +internal/ceres/dense_jacobian_writer.h +internal/ceres/dense_qr_solver.cc +internal/ceres/dense_qr_solver.h +internal/ceres/dense_sparse_matrix.cc +internal/ceres/dense_sparse_matrix.h +internal/ceres/detect_structure.cc +internal/ceres/detect_structure.h +internal/ceres/evaluator.cc +internal/ceres/evaluator.h +internal/ceres/file.cc +internal/ceres/file.h +internal/ceres/generated/schur_eliminator_2_2_2.cc +internal/ceres/generated/schur_eliminator_2_2_3.cc +internal/ceres/generated/schur_eliminator_2_2_4.cc +internal/ceres/generated/schur_eliminator_2_2_d.cc +internal/ceres/generated/schur_eliminator_2_3_3.cc +internal/ceres/generated/schur_eliminator_2_3_4.cc +internal/ceres/generated/schur_eliminator_2_3_9.cc +internal/ceres/generated/schur_eliminator_2_3_d.cc +internal/ceres/generated/schur_eliminator_2_4_3.cc +internal/ceres/generated/schur_eliminator_2_4_4.cc +internal/ceres/generated/schur_eliminator_2_4_d.cc +internal/ceres/generated/schur_eliminator_4_4_2.cc +internal/ceres/generated/schur_eliminator_4_4_3.cc +internal/ceres/generated/schur_eliminator_4_4_4.cc +internal/ceres/generated/schur_eliminator_4_4_d.cc +internal/ceres/generated/schur_eliminator_d_d_d.cc +internal/ceres/gradient_checking_cost_function.cc +internal/ceres/gradient_checking_cost_function.h +internal/ceres/graph_algorithms.h +internal/ceres/graph.h +internal/ceres/implicit_schur_complement.cc +internal/ceres/implicit_schur_complement.h +internal/ceres/integral_types.h +internal/ceres/iterative_schur_complement_solver.cc +internal/ceres/iterative_schur_complement_solver.h +internal/ceres/levenberg_marquardt.cc +internal/ceres/levenberg_marquardt.h +internal/ceres/linear_least_squares_problems.cc +internal/ceres/linear_least_squares_problems.h +internal/ceres/linear_operator.cc +internal/ceres/linear_operator.h +internal/ceres/linear_solver.cc +internal/ceres/linear_solver.h +internal/ceres/local_parameterization.cc +internal/ceres/loss_function.cc +internal/ceres/map_util.h +internal/ceres/matrix_proto.h +internal/ceres/minimizer.h +internal/ceres/mutex.h +internal/ceres/normal_prior.cc +internal/ceres/parameter_block.h +internal/ceres/partitioned_matrix_view.cc +internal/ceres/partitioned_matrix_view.h +internal/ceres/problem.cc +internal/ceres/problem_impl.cc +internal/ceres/problem_impl.h +internal/ceres/program.cc +internal/ceres/program_evaluator.h +internal/ceres/program.h +internal/ceres/random.h +internal/ceres/residual_block.cc +internal/ceres/residual_block.h +internal/ceres/residual_block_utils.cc +internal/ceres/residual_block_utils.h +internal/ceres/runtime_numeric_diff_cost_function.cc +internal/ceres/runtime_numeric_diff_cost_function.h +internal/ceres/schur_complement_solver.cc +internal/ceres/schur_complement_solver.h +internal/ceres/schur_eliminator.cc +internal/ceres/schur_eliminator.h +internal/ceres/schur_eliminator_impl.h +internal/ceres/schur_ordering.cc +internal/ceres/schur_ordering.h +internal/ceres/scratch_evaluate_preparer.cc +internal/ceres/scratch_evaluate_preparer.h +internal/ceres/solver.cc +internal/ceres/solver_impl.cc +internal/ceres/solver_impl.h +internal/ceres/sparse_matrix.cc +internal/ceres/sparse_matrix.h +internal/ceres/sparse_normal_cholesky_solver.cc +internal/ceres/sparse_normal_cholesky_solver.h +internal/ceres/split.cc +internal/ceres/stl_util.h +internal/ceres/stringprintf.cc +internal/ceres/stringprintf.h +internal/ceres/suitesparse.cc +internal/ceres/suitesparse.h +internal/ceres/triplet_sparse_matrix.cc +internal/ceres/triplet_sparse_matrix.h +internal/ceres/types.cc +internal/ceres/visibility_based_preconditioner.cc +internal/ceres/visibility_based_preconditioner.h +internal/ceres/visibility.cc +internal/ceres/visibility.h diff --git a/extern/libmv/third_party/ceres/include/ceres/autodiff_cost_function.h b/extern/libmv/third_party/ceres/include/ceres/autodiff_cost_function.h new file mode 100644 index 00000000000..e86d6993864 --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/autodiff_cost_function.h @@ -0,0 +1,211 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Helpers for making CostFunctions as needed by the least squares framework, +// with Jacobians computed via automatic differentiation. For more information +// on automatic differentation, see the wikipedia article at +// http://en.wikipedia.org/wiki/Automatic_differentiation +// +// To get an auto differentiated cost function, you must define a class with a +// templated operator() (a functor) that computes the cost function in terms of +// the template parameter T. The autodiff framework substitutes appropriate +// "jet" objects for T in order to compute the derivative when necessary, but +// this is hidden, and you should write the function as if T were a scalar type +// (e.g. a double-precision floating point number). +// +// The function must write the computed value in the last argument (the only +// non-const one) and return true to indicate success. +// +// For example, consider a scalar error e = k - x'y, where both x and y are +// two-dimensional column vector parameters, the prime sign indicates +// transposition, and k is a constant. The form of this error, which is the +// difference between a constant and an expression, is a common pattern in least +// squares problems. For example, the value x'y might be the model expectation +// for a series of measurements, where there is an instance of the cost function +// for each measurement k. +// +// The actual cost added to the total problem is e^2, or (k - x'k)^2; however, +// the squaring is implicitly done by the optimization framework. +// +// To write an auto-differentiable cost function for the above model, first +// define the object +// +// class MyScalarCostFunction { +// MyScalarCostFunction(double k): k_(k) {} +// +// template <typename T> +// bool operator()(const T* const x , const T* const y, T* e) const { +// e[0] = T(k_) - x[0] * y[0] + x[1] * y[1]; +// return true; +// } +// +// private: +// double k_; +// }; +// +// Note that in the declaration of operator() the input parameters x and y come +// first, and are passed as const pointers to arrays of T. If there were three +// input parameters, then the third input parameter would come after y. The +// output is always the last parameter, and is also a pointer to an array. In +// the example above, e is a scalar, so only e[0] is set. +// +// Then given this class definition, the auto differentiated cost function for +// it can be constructed as follows. +// +// CostFunction* cost_function +// = new AutoDiffCostFunction<MyScalarCostFunction, 1, 2, 2>( +// new MyScalarCostFunction(1.0)); ^ ^ ^ +// | | | +// Dimension of residual ------+ | | +// Dimension of x ----------------+ | +// Dimension of y -------------------+ +// +// In this example, there is usually an instance for each measumerent of k. +// +// In the instantiation above, the template parameters following +// "MyScalarCostFunction", "1, 2, 2", describe the functor as computing a +// 1-dimensional output from two arguments, both 2-dimensional. +// +// The autodiff cost function also supports cost functions with a +// runtime-determined number of residuals. For example: +// +// CostFunction* cost_function +// = new AutoDiffCostFunction<MyScalarCostFunction, DYNAMIC, 2, 2>( +// new CostFunctionWithDynamicNumResiduals(1.0), ^ ^ ^ +// runtime_number_of_residuals); <----+ | | | +// | | | | +// | | | | +// Actual number of residuals ------+ | | | +// Indicate dynamic number of residuals ---------+ | | +// Dimension of x -------------------------------------+ | +// Dimension of y ----------------------------------------+ +// +// The framework can currently accommodate cost functions of up to 6 independent +// variables, and there is no limit on the dimensionality of each of them. +// +// WARNING #1: Since the functor will get instantiated with different types for +// T, you must to convert from other numeric types to T before mixing +// computations with other variables of type T. In the example above, this is +// seen where instead of using k_ directly, k_ is wrapped with T(k_). +// +// WARNING #2: A common beginner's error when first using autodiff cost +// functions is to get the sizing wrong. In particular, there is a tendency to +// set the template parameters to (dimension of residual, number of parameters) +// instead of passing a dimension parameter for *every parameter*. In the +// example above, that would be <MyScalarCostFunction, 1, 2>, which is missing +// the last '2' argument. Please be careful when setting the size parameters. + +#ifndef CERES_PUBLIC_AUTODIFF_COST_FUNCTION_H_ +#define CERES_PUBLIC_AUTODIFF_COST_FUNCTION_H_ + +#include <glog/logging.h> +#include "ceres/internal/autodiff.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/sized_cost_function.h" +#include "ceres/types.h" + +namespace ceres { + +// A cost function which computes the derivative of the cost with respect to +// the parameters (a.k.a. the jacobian) using an autodifferentiation framework. +// The first template argument is the functor object, described in the header +// comment. The second argument is the dimension of the residual (or +// ceres::DYNAMIC to indicate it will be set at runtime), and subsequent +// arguments describe the size of the Nth parameter, one per parameter. +// +// The constructors take ownership of the cost functor. +// +// If the number of residuals (argument "M" below) is ceres::DYNAMIC, then the +// two-argument constructor must be used. The second constructor takes a number +// of residuals (in addition to the templated number of residuals). This allows +// for varying the number of residuals for a single autodiff cost function at +// runtime. +template <typename CostFunctor, + int M, // Number of residuals, or ceres::DYNAMIC. + int N0, // Number of parameters in block 0. + int N1 = 0, // Number of parameters in block 1. + int N2 = 0, // Number of parameters in block 2. + int N3 = 0, // Number of parameters in block 3. + int N4 = 0, // Number of parameters in block 4. + int N5 = 0> // Number of parameters in block 5. +class AutoDiffCostFunction : + public SizedCostFunction<M, N0, N1, N2, N3, N4, N5> { + public: + // Takes ownership of functor. Uses the template-provided value for the + // number of residuals ("M"). + explicit AutoDiffCostFunction(CostFunctor* functor) + : functor_(functor) { + CHECK_NE(M, DYNAMIC) << "Can't run the fixed-size constructor if the " + << "number of residuals is set to ceres::DYNAMIC."; + } + + // Takes ownership of functor. Ignores the template-provided number of + // residuals ("M") in favor of the "num_residuals" argument provided. + // + // This allows for having autodiff cost functions which return varying + // numbers of residuals at runtime. + AutoDiffCostFunction(CostFunctor* functor, int num_residuals) + : functor_(functor) { + CHECK_EQ(M, DYNAMIC) << "Can't run the dynamic-size constructor if the " + << "number of residuals is not ceres::DYNAMIC."; + SizedCostFunction<M, N0, N1, N2, N3, N4, N5>::set_num_residuals(num_residuals); + } + + virtual ~AutoDiffCostFunction() {} + + // Implementation details follow; clients of the autodiff cost function should + // not have to examine below here. + // + // To handle varardic cost functions, some template magic is needed. It's + // mostly hidden inside autodiff.h. + virtual bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const { + if (!jacobians) { + return internal::VariadicEvaluate< + CostFunctor, double, N0, N1, N2, N3, N4, N5> + ::Call(*functor_, parameters, residuals); + } + return internal::AutoDiff<CostFunctor, double, + N0, N1, N2, N3, N4, N5>::Differentiate( + *functor_, + parameters, + SizedCostFunction<M, N0, N1, N2, N3, N4, N5>::num_residuals(), + residuals, + jacobians); + } + + private: + internal::scoped_ptr<CostFunctor> functor_; +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_AUTODIFF_COST_FUNCTION_H_ diff --git a/extern/libmv/third_party/ceres/include/ceres/ceres.h b/extern/libmv/third_party/ceres/include/ceres/ceres.h new file mode 100644 index 00000000000..22aaf8ff21a --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/ceres.h @@ -0,0 +1,48 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// This is a forwarding header containing the public symbols exported from +// Ceres. Anything in the "ceres" namespace is available for use. + +#ifndef CERES_PUBLIC_CERES_H_ +#define CERES_PUBLIC_CERES_H_ + +#include "ceres/autodiff_cost_function.h" +#include "ceres/cost_function.h" +#include "ceres/iteration_callback.h" +#include "ceres/local_parameterization.h" +#include "ceres/loss_function.h" +#include "ceres/numeric_diff_cost_function.h" +#include "ceres/problem.h" +#include "ceres/sized_cost_function.h" +#include "ceres/solver.h" +#include "ceres/types.h" + +#endif // CERES_PUBLIC_CERES_H_ diff --git a/extern/libmv/third_party/ceres/include/ceres/conditioned_cost_function.h b/extern/libmv/third_party/ceres/include/ceres/conditioned_cost_function.h new file mode 100644 index 00000000000..498d36ee55a --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/conditioned_cost_function.h @@ -0,0 +1,97 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: wjr@google.com (William Rucklidge) +// +// This file contains a cost function that can apply a transformation to +// each residual value before they are square-summed. + +#ifndef CERES_PUBLIC_CONDITIONED_COST_FUNCTION_H_ +#define CERES_PUBLIC_CONDITIONED_COST_FUNCTION_H_ + +#include <vector> + +#include "ceres/cost_function.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { + +// This class allows you to apply different conditioning to the residual +// values of a wrapped cost function. An example where this is useful is +// where you have an existing cost function that produces N values, but you +// want the total cost to be something other than just the sum of these +// squared values - maybe you want to apply a different scaling to some +// values, to change their contribution to the cost. +// +// Usage: +// +// // my_cost_function produces N residuals +// CostFunction* my_cost_function = ... +// CHECK_EQ(N, my_cost_function->num_residuals()); +// vector<CostFunction*> conditioners; +// +// // Make N 1x1 cost functions (1 parameter, 1 residual) +// CostFunction* f_1 = ... +// conditioners.push_back(f_1); +// ... +// CostFunction* f_N = ... +// conditioners.push_back(f_N); +// ConditionedCostFunction* ccf = +// new ConditionedCostFunction(my_cost_function, conditioners); +// +// Now ccf's residual i (i=0..N-1) will be passed though the i'th conditioner. +// +// ccf_residual[i] = f_i(my_cost_function_residual[i]) +// +// and the Jacobian will be affected appropriately. +class ConditionedCostFunction : public CostFunction { + public: + // Builds a cost function based on a wrapped cost function, and a + // per-residual conditioner. Takes ownership of all of the wrapped cost + // functions, or not, depending on the ownership parameter. Conditioners + // may be NULL, in which case the corresponding residual is not modified. + ConditionedCostFunction(CostFunction* wrapped_cost_function, + const vector<CostFunction*>& conditioners, + Ownership ownership); + virtual ~ConditionedCostFunction(); + + virtual bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const; + + private: + internal::scoped_ptr<CostFunction> wrapped_cost_function_; + vector<CostFunction*> conditioners_; + Ownership ownership_; +}; + +} // namespace ceres + + +#endif // CERES_PUBLIC_CONDITIONED_COST_FUNCTION_H_ diff --git a/extern/libmv/third_party/ceres/include/ceres/cost_function.h b/extern/libmv/third_party/ceres/include/ceres/cost_function.h new file mode 100644 index 00000000000..84403d90636 --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/cost_function.h @@ -0,0 +1,127 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// keir@google.m (Keir Mierle) +// +// This is the interface through which the least squares solver accesses the +// residual and Jacobian of the least squares problem. Users are expected to +// subclass CostFunction to define their own terms in the least squares problem. +// +// It is recommended that users define templated residual functors for use as +// arguments for AutoDiffCostFunction (see autodiff_cost_function.h), instead of +// directly implementing the CostFunction interface. This often results in both +// shorter code and faster execution than hand-coded derivatives. However, +// specialized cases may demand direct implementation of the lower-level +// CostFunction interface; for example, this is true when calling legacy code +// which is not templated on numeric types. + +#ifndef CERES_PUBLIC_COST_FUNCTION_H_ +#define CERES_PUBLIC_COST_FUNCTION_H_ + +#include <vector> +#include "ceres/internal/macros.h" +#include "ceres/internal/port.h" +#include "ceres/types.h" + +namespace ceres { + +// This class implements the computation of the cost (a.k.a. residual) terms as +// a function of the input (control) variables, and is the interface for users +// to describe their least squares problem to Ceres. In other words, this is the +// modelling layer between users and the Ceres optimizer. The signature of the +// function (number and sizes of input parameter blocks and number of outputs) +// is stored in parameter_block_sizes_ and num_residuals_ respectively. User +// code inheriting from this class is expected to set these two members with the +// corresponding accessors. This information will be verified by the Problem +// when added with AddResidualBlock(). +class CostFunction { + public: + CostFunction() : num_residuals_(0) {} + + virtual ~CostFunction() {} + + // Inputs: + // + // parameters is an array of pointers to arrays containing the + // various parameter blocks. parameters has the same number of + // elements as parameter_block_sizes_. Parameter blocks are in the + // same order as parameter_block_sizes_.i.e., + // + // parameters_[i] = double[parameter_block_sizes_[i]] + // + // Outputs: + // + // residuals is an array of size num_residuals_. + // + // jacobians is an array of size parameter_block_sizes_ containing + // pointers to storage for jacobian blocks corresponding to each + // parameter block. Jacobian blocks are in the same order as + // parameter_block_sizes, i.e. jacobians[i], is an + // array that contains num_residuals_* parameter_block_sizes_[i] + // elements. Each jacobian block is stored in row-major order, i.e., + // + // jacobians[i][r*parameter_block_size_[i] + c] = + // d residual[r] / d parameters[i][c] + // + // If jacobians is NULL, then no derivatives are returned; this is + // the case when computing cost only. If jacobians[i] is NULL, then + // the jacobian block corresponding to the i'th parameter block must + // not to be returned. + virtual bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const = 0; + + const vector<int16>& parameter_block_sizes() const { + return parameter_block_sizes_; + } + + int num_residuals() const { + return num_residuals_; + } + + protected: + vector<int16>* mutable_parameter_block_sizes() { + return ¶meter_block_sizes_; + } + + void set_num_residuals(int num_residuals) { + num_residuals_ = num_residuals; + } + + private: + // Cost function signature metadata: number of inputs & their sizes, + // number of outputs (residuals). + vector<int16> parameter_block_sizes_; + int num_residuals_; + DISALLOW_COPY_AND_ASSIGN(CostFunction); +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_COST_FUNCTION_H_ diff --git a/extern/libmv/third_party/ceres/include/ceres/internal/autodiff.h b/extern/libmv/third_party/ceres/include/ceres/internal/autodiff.h new file mode 100644 index 00000000000..4f5081f8f66 --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/internal/autodiff.h @@ -0,0 +1,370 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// Computation of the Jacobian matrix for vector-valued functions of multiple +// variables, using automatic differentiation based on the implementation of +// dual numbers in jet.h. Before reading the rest of this file, it is adivsable +// to read jet.h's header comment in detail. +// +// The helper wrapper AutoDiff::Differentiate() computes the jacobian of +// functors with templated operator() taking this form: +// +// struct F { +// template<typename T> +// bool operator(const T *x, const T *y, ..., T *z) { +// // Compute z[] based on x[], y[], ... +// // return true if computation succeeded, false otherwise. +// } +// }; +// +// All inputs and outputs may be vector-valued. +// +// To understand how jets are used to compute the jacobian, a +// picture may help. Consider a vector-valued function, F, returning 3 +// dimensions and taking a vector-valued parameter of 4 dimensions: +// +// y x +// [ * ] F [ * ] +// [ * ] <--- [ * ] +// [ * ] [ * ] +// [ * ] +// +// Similar to the 2-parameter example for f described in jet.h, computing the +// jacobian dy/dx is done by substutiting a suitable jet object for x and all +// intermediate steps of the computation of F. Since x is has 4 dimensions, use +// a Jet<double, 4>. +// +// Before substituting a jet object for x, the dual components are set +// appropriately for each dimension of x: +// +// y x +// [ * | * * * * ] f [ * | 1 0 0 0 ] x0 +// [ * | * * * * ] <--- [ * | 0 1 0 0 ] x1 +// [ * | * * * * ] [ * | 0 0 1 0 ] x2 +// ---+--- [ * | 0 0 0 1 ] x3 +// | ^ ^ ^ ^ +// dy/dx | | | +----- infinitesimal for x3 +// | | +------- infinitesimal for x2 +// | +--------- infinitesimal for x1 +// +----------- infinitesimal for x0 +// +// The reason to set the internal 4x4 submatrix to the identity is that we wish +// to take the derivative of y separately with respect to each dimension of x. +// Each column of the 4x4 identity is therefore for a single component of the +// independent variable x. +// +// Then the jacobian of the mapping, dy/dx, is the 3x4 sub-matrix of the +// extended y vector, indicated in the above diagram. +// +// Functors with multiple parameters +// --------------------------------- +// In practice, it is often convenient to use a function f of two or more +// vector-valued parameters, for example, x[3] and z[6]. Unfortunately, the jet +// framework is designed for a single-parameter vector-valued input. The wrapper +// in this file addresses this issue adding support for functions with one or +// more parameter vectors. +// +// To support multiple parameters, all the parameter vectors are concatenated +// into one and treated as a single parameter vector, except that since the +// functor expects different inputs, we need to construct the jets as if they +// were part of a single parameter vector. The extended jets are passed +// separately for each parameter. +// +// For example, consider a functor F taking two vector parameters, p[2] and +// q[3], and producing an output y[4]: +// +// struct F { +// template<typename T> +// bool operator(const T *p, const T *q, T *z) { +// // ... +// } +// }; +// +// In this case, the necessary jet type is Jet<double, 5>. Here is a +// visualization of the jet objects in this case: +// +// Dual components for p ----+ +// | +// -+- +// y [ * | 1 0 | 0 0 0 ] --- p[0] +// [ * | 0 1 | 0 0 0 ] --- p[1] +// [ * | . . | + + + ] | +// [ * | . . | + + + ] v +// [ * | . . | + + + ] <--- F(p, q) +// [ * | . . | + + + ] ^ +// ^^^ ^^^^^ | +// dy/dp dy/dq [ * | 0 0 | 1 0 0 ] --- q[0] +// [ * | 0 0 | 0 1 0 ] --- q[1] +// [ * | 0 0 | 0 0 1 ] --- q[2] +// --+-- +// | +// Dual components for q --------------+ +// +// where the 4x2 submatrix (marked with ".") and 4x3 submatrix (marked with "+" +// of y in the above diagram are the derivatives of y with respect to p and q +// respectively. This is how autodiff works for functors taking multiple vector +// valued arguments (up to 6). +// +// Jacobian NULL pointers +// ---------------------- +// In general, the functions below will accept NULL pointers for all or some of +// the Jacobian parameters, meaning that those Jacobians will not be computed. + +#ifndef CERES_PUBLIC_INTERNAL_AUTODIFF_H_ +#define CERES_PUBLIC_INTERNAL_AUTODIFF_H_ + +#include <stddef.h> + +#include <glog/logging.h> +#include "ceres/jet.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/fixed_array.h" + +namespace ceres { +namespace internal { + +// Extends src by a 1st order pertubation for every dimension and puts it in +// dst. The size of src is N. Since this is also used for perturbations in +// blocked arrays, offset is used to shift which part of the jet the +// perturbation occurs. This is used to set up the extended x augmented by an +// identity matrix. The JetT type should be a Jet type, and T should be a +// numeric type (e.g. double). For example, +// +// 0 1 2 3 4 5 6 7 8 +// dst[0] [ * | . . | 1 0 0 | . . . ] +// dst[1] [ * | . . | 0 1 0 | . . . ] +// dst[2] [ * | . . | 0 0 1 | . . . ] +// +// is what would get put in dst if N was 3, offset was 3, and the jet type JetT +// was 8-dimensional. +template <typename JetT, typename T> +inline void Make1stOrderPerturbation(int offset, int N, const T *src, + JetT *dst) { + DCHECK(src); + DCHECK(dst); + for (int j = 0; j < N; ++j) { + dst[j] = JetT(src[j], offset + j); + } +} + +// Takes the 0th order part of src, assumed to be a Jet type, and puts it in +// dst. This is used to pick out the "vector" part of the extended y. +template <typename JetT, typename T> +inline void Take0thOrderPart(int M, const JetT *src, T dst) { + DCHECK(src); + for (int i = 0; i < M; ++i) { + dst[i] = src[i].a; + } +} + +// Takes N 1st order parts, starting at index N0, and puts them in the M x N +// matrix 'dst'. This is used to pick out the "matrix" parts of the extended y. +template <typename JetT, typename T, int N0, int N> +inline void Take1stOrderPart(const int M, const JetT *src, T *dst) { + DCHECK(src); + DCHECK(dst); + for (int i = 0; i < M; ++i) { + Eigen::Map<Eigen::Matrix<T, N, 1> >(dst + N * i, N) = src[i].v.template segment<N>(N0); + } +} + +// This block of quasi-repeated code calls the user-supplied functor, which may +// take a variable number of arguments. This is accomplished by specializing the +// struct based on the size of the trailing parameters; parameters with 0 size +// are assumed missing. +// +// Supporting variadic functions is the primary source of complexity in the +// autodiff implementation. + +template<typename Functor, typename T, + int N0, int N1, int N2, int N3, int N4, int N5> +struct VariadicEvaluate { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + input[2], + input[3], + input[4], + input[5], + output); + } +}; + +template<typename Functor, typename T, + int N0, int N1, int N2, int N3, int N4> +struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, 0> { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + input[2], + input[3], + input[4], + output); + } +}; + +template<typename Functor, typename T, + int N0, int N1, int N2, int N3> +struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, 0, 0> { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + input[2], + input[3], + output); + } +}; + +template<typename Functor, typename T, + int N0, int N1, int N2> +struct VariadicEvaluate<Functor, T, N0, N1, N2, 0, 0, 0> { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + input[2], + output); + } +}; + +template<typename Functor, typename T, + int N0, int N1> +struct VariadicEvaluate<Functor, T, N0, N1, 0, 0, 0, 0> { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + input[1], + output); + } +}; + +template<typename Functor, typename T, int N0> +struct VariadicEvaluate<Functor, T, N0, 0, 0, 0, 0, 0> { + static bool Call(const Functor& functor, T const *const *input, T* output) { + return functor(input[0], + output); + } +}; + +// This is in a struct because default template parameters on a function are not +// supported in C++03 (though it is available in C++0x). N0 through N5 are the +// dimension of the input arguments to the user supplied functor. +template <typename Functor, typename T, + int N0 = 0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0, int N5=0> +struct AutoDiff { + static bool Differentiate(const Functor& functor, + T const *const *parameters, + int num_outputs, + T *function_value, + T **jacobians) { + typedef Jet<T, N0 + N1 + N2 + N3 + N4 + N5> JetT; + + DCHECK_GT(N0, 0) + << "Cost functions must have at least one parameter block."; + DCHECK((!N1 && !N2 && !N3 && !N4 && !N5) || + ((N1 > 0) && !N2 && !N3 && !N4 && !N5) || + ((N1 > 0) && (N2 > 0) && !N3 && !N4 && !N5) || + ((N1 > 0) && (N2 > 0) && (N3 > 0) && !N4 && !N5) || + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && !N5) || + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0))) + << "Zero block cannot precede a non-zero block. Block sizes are " + << "(ignore trailing 0s): " << N0 << ", " << N1 << ", " << N2 << ", " + << N3 << ", " << N4 << ", " << N5; + + DCHECK_GT(num_outputs, 0); + + FixedArray<JetT, (256 * 7) / sizeof(JetT)> x( + N0 + N1 + N2 + N3 + N4 + N5 + num_outputs); + + // It's ugly, but it works. + const int jet0 = 0; + const int jet1 = N0; + const int jet2 = N0 + N1; + const int jet3 = N0 + N1 + N2; + const int jet4 = N0 + N1 + N2 + N3; + const int jet5 = N0 + N1 + N2 + N3 + N4; + const int jet6 = N0 + N1 + N2 + N3 + N4 + N5; + + const JetT *unpacked_parameters[6] = { + x.get() + jet0, + x.get() + jet1, + x.get() + jet2, + x.get() + jet3, + x.get() + jet4, + x.get() + jet5, + }; + JetT *output = x.get() + jet6; + +#define CERES_MAKE_1ST_ORDER_PERTURBATION(i) \ + if (N ## i) { \ + internal::Make1stOrderPerturbation(jet ## i, \ + N ## i, \ + parameters[i], \ + x.get() + jet ## i); \ + } + CERES_MAKE_1ST_ORDER_PERTURBATION(0); + CERES_MAKE_1ST_ORDER_PERTURBATION(1); + CERES_MAKE_1ST_ORDER_PERTURBATION(2); + CERES_MAKE_1ST_ORDER_PERTURBATION(3); + CERES_MAKE_1ST_ORDER_PERTURBATION(4); + CERES_MAKE_1ST_ORDER_PERTURBATION(5); +#undef CERES_MAKE_1ST_ORDER_PERTURBATION + + if (!VariadicEvaluate<Functor, JetT, + N0, N1, N2, N3, N4, N5>::Call( + functor, unpacked_parameters, output)) { + return false; + } + + internal::Take0thOrderPart(num_outputs, output, function_value); + +#define CERES_TAKE_1ST_ORDER_PERTURBATION(i) \ + if (N ## i) { \ + if (jacobians[i]) { \ + internal::Take1stOrderPart<JetT, T, \ + jet ## i, \ + N ## i>(num_outputs, \ + output, \ + jacobians[i]); \ + } \ + } + CERES_TAKE_1ST_ORDER_PERTURBATION(0); + CERES_TAKE_1ST_ORDER_PERTURBATION(1); + CERES_TAKE_1ST_ORDER_PERTURBATION(2); + CERES_TAKE_1ST_ORDER_PERTURBATION(3); + CERES_TAKE_1ST_ORDER_PERTURBATION(4); + CERES_TAKE_1ST_ORDER_PERTURBATION(5); +#undef CERES_TAKE_1ST_ORDER_PERTURBATION + return true; + } +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_PUBLIC_INTERNAL_AUTODIFF_H_ diff --git a/extern/libmv/third_party/ceres/include/ceres/internal/eigen.h b/extern/libmv/third_party/ceres/include/ceres/internal/eigen.h new file mode 100644 index 00000000000..be76f9eff98 --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/internal/eigen.h @@ -0,0 +1,80 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_EIGEN_H_ +#define CERES_INTERNAL_EIGEN_H_ + +#include "Eigen/Core" + +namespace ceres { + +using Eigen::Dynamic; +using Eigen::RowMajor; + +typedef Eigen::Matrix<double, Dynamic, 1> Vector; +typedef Eigen::Matrix<double, Dynamic, Dynamic, RowMajor> Matrix; +typedef Eigen::Map<Vector> VectorRef; +typedef Eigen::Map<Matrix> MatrixRef; +typedef Eigen::Map<Matrix, Eigen::Aligned> AlignedMatrixRef; +typedef Eigen::Map<const Vector> ConstVectorRef; +typedef Eigen::Map<const Matrix, Eigen::Aligned> ConstAlignedMatrixRef; +typedef Eigen::Map<const Matrix> ConstMatrixRef; + +// C++ does not support templated typdefs, thus the need for this +// struct so that we can support statically sized Matrix and Maps. +template <int num_rows = Eigen::Dynamic, int num_cols = Eigen::Dynamic> +struct EigenTypes { + typedef Eigen::Matrix <double, num_rows, num_cols, RowMajor> + Matrix; + + typedef Eigen::Map< + Eigen::Matrix<double, num_rows, num_cols, RowMajor> > + MatrixRef; + + typedef Eigen::Matrix <double, num_rows, 1> + Vector; + + typedef Eigen::Map < + Eigen::Matrix<double, num_rows, 1> > + VectorRef; + + + typedef Eigen::Map< + const Eigen::Matrix<double, num_rows, num_cols, RowMajor> > + ConstMatrixRef; + + typedef Eigen::Map < + const Eigen::Matrix<double, num_rows, 1> > + ConstVectorRef; +}; + +} // namespace ceres + +#endif // CERES_INTERNAL_EIGEN_H_ diff --git a/extern/libmv/third_party/ceres/include/ceres/internal/fixed_array.h b/extern/libmv/third_party/ceres/include/ceres/internal/fixed_array.h new file mode 100644 index 00000000000..84617c4fa06 --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/internal/fixed_array.h @@ -0,0 +1,194 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: rennie@google.com (Jeffrey Rennie) +// Author: sanjay@google.com (Sanjay Ghemawat) -- renamed to FixedArray + +#ifndef CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_ +#define CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_ + +#include <cstddef> +#include <glog/logging.h> +#include "ceres/internal/manual_constructor.h" + +namespace ceres { +namespace internal { + +// A FixedArray<T> represents a non-resizable array of T where the +// length of the array does not need to be a compile time constant. +// +// FixedArray allocates small arrays inline, and large arrays on +// the heap. It is a good replacement for non-standard and deprecated +// uses of alloca() and variable length arrays (a GCC extension). +// +// FixedArray keeps performance fast for small arrays, because it +// avoids heap operations. It also helps reduce the chances of +// accidentally overflowing your stack if large input is passed to +// your function. +// +// Also, FixedArray is useful for writing portable code. Not all +// compilers support arrays of dynamic size. + +// Most users should not specify an inline_elements argument and let +// FixedArray<> automatically determine the number of elements +// to store inline based on sizeof(T). +// +// If inline_elements is specified, the FixedArray<> implementation +// will store arrays of length <= inline_elements inline. +// +// Finally note that unlike vector<T> FixedArray<T> will not zero-initialize +// simple types like int, double, bool, etc. +// +// Non-POD types will be default-initialized just like regular vectors or +// arrays. + +#if defined(_WIN64) + typedef __int64 ssize_t; +#elif defined(_WIN32) + typedef __int32 ssize_t; +#endif + +template <typename T, ssize_t inline_elements = -1> +class FixedArray { + public: + // For playing nicely with stl: + typedef T value_type; + typedef T* iterator; + typedef T const* const_iterator; + typedef T& reference; + typedef T const& const_reference; + typedef T* pointer; + typedef std::ptrdiff_t difference_type; + typedef size_t size_type; + + // REQUIRES: n >= 0 + // Creates an array object that can store "n" elements. + // + // FixedArray<T> will not zero-initialiaze POD (simple) types like int, + // double, bool, etc. + // Non-POD types will be default-initialized just like regular vectors or + // arrays. + explicit FixedArray(size_type n); + + // Releases any resources. + ~FixedArray(); + + // Returns the length of the array. + inline size_type size() const { return size_; } + + // Returns the memory size of the array in bytes. + inline size_t memsize() const { return size_ * sizeof(T); } + + // Returns a pointer to the underlying element array. + inline const T* get() const { return &array_[0].element; } + inline T* get() { return &array_[0].element; } + + // REQUIRES: 0 <= i < size() + // Returns a reference to the "i"th element. + inline T& operator[](size_type i) { + DCHECK_GE(i, 0); + DCHECK_LT(i, size_); + return array_[i].element; + } + + // REQUIRES: 0 <= i < size() + // Returns a reference to the "i"th element. + inline const T& operator[](size_type i) const { + DCHECK_GE(i, 0); + DCHECK_LT(i, size_); + return array_[i].element; + } + + inline iterator begin() { return &array_[0].element; } + inline iterator end() { return &array_[size_].element; } + + inline const_iterator begin() const { return &array_[0].element; } + inline const_iterator end() const { return &array_[size_].element; } + + private: + // Container to hold elements of type T. This is necessary to handle + // the case where T is a a (C-style) array. The size of InnerContainer + // and T must be the same, otherwise callers' assumptions about use + // of this code will be broken. + struct InnerContainer { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + T element; + }; + + // How many elements should we store inline? + // a. If not specified, use a default of 256 bytes (256 bytes + // seems small enough to not cause stack overflow or unnecessary + // stack pollution, while still allowing stack allocation for + // reasonably long character arrays. + // b. Never use 0 length arrays (not ISO C++) + static const size_type S1 = ((inline_elements < 0) + ? (256/sizeof(T)) : inline_elements); + static const size_type S2 = (S1 <= 0) ? 1 : S1; + static const size_type kInlineElements = S2; + + size_type const size_; + InnerContainer* const array_; + + // Allocate some space, not an array of elements of type T, so that we can + // skip calling the T constructors and destructors for space we never use. + ManualConstructor<InnerContainer> inline_space_[kInlineElements]; +}; + +// Implementation details follow + +template <class T, ssize_t S> +inline FixedArray<T, S>::FixedArray(typename FixedArray<T, S>::size_type n) + : size_(n), + array_((n <= kInlineElements + ? reinterpret_cast<InnerContainer*>(inline_space_) + : new InnerContainer[n])) { + DCHECK_GE(n, 0); + + // Construct only the elements actually used. + if (array_ == reinterpret_cast<InnerContainer*>(inline_space_)) { + for (int i = 0; i != size_; ++i) { + inline_space_[i].Init(); + } + } +} + +template <class T, ssize_t S> +inline FixedArray<T, S>::~FixedArray() { + if (array_ != reinterpret_cast<InnerContainer*>(inline_space_)) { + delete[] array_; + } else { + for (int i = 0; i != size_; ++i) { + inline_space_[i].Destroy(); + } + } +} + +} // namespace internal +} // namespace ceres + +#endif // CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_ diff --git a/extern/libmv/third_party/ceres/include/ceres/internal/macros.h b/extern/libmv/third_party/ceres/include/ceres/internal/macros.h new file mode 100644 index 00000000000..0cfd773bcca --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/internal/macros.h @@ -0,0 +1,154 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// +// Various Google-specific macros. +// +// This code is compiled directly on many platforms, including client +// platforms like Windows, Mac, and embedded systems. Before making +// any changes here, make sure that you're not breaking any platforms. + +#ifndef CERES_PUBLIC_INTERNAL_MACROS_H_ +#define CERES_PUBLIC_INTERNAL_MACROS_H_ + +#include <cstddef> // For size_t. + +// A macro to disallow the copy constructor and operator= functions +// This should be used in the private: declarations for a class +// +// For disallowing only assign or copy, write the code directly, but declare +// the intend in a comment, for example: +// void operator=(const TypeName&); // DISALLOW_ASSIGN +// Note, that most uses of DISALLOW_ASSIGN and DISALLOW_COPY are broken +// semantically, one should either use disallow both or neither. Try to +// avoid these in new code. +#define DISALLOW_COPY_AND_ASSIGN(TypeName) \ + TypeName(const TypeName&); \ + void operator=(const TypeName&) + +// A macro to disallow all the implicit constructors, namely the +// default constructor, copy constructor and operator= functions. +// +// This should be used in the private: declarations for a class +// that wants to prevent anyone from instantiating it. This is +// especially useful for classes containing only static methods. +#define DISALLOW_IMPLICIT_CONSTRUCTORS(TypeName) \ + TypeName(); \ + DISALLOW_COPY_AND_ASSIGN(TypeName) + +// The arraysize(arr) macro returns the # of elements in an array arr. +// The expression is a compile-time constant, and therefore can be +// used in defining new arrays, for example. If you use arraysize on +// a pointer by mistake, you will get a compile-time error. +// +// One caveat is that arraysize() doesn't accept any array of an +// anonymous type or a type defined inside a function. In these rare +// cases, you have to use the unsafe ARRAYSIZE() macro below. This is +// due to a limitation in C++'s template system. The limitation might +// eventually be removed, but it hasn't happened yet. + +// This template function declaration is used in defining arraysize. +// Note that the function doesn't need an implementation, as we only +// use its type. +template <typename T, size_t N> +char (&ArraySizeHelper(T (&array)[N]))[N]; + +// That gcc wants both of these prototypes seems mysterious. VC, for +// its part, can't decide which to use (another mystery). Matching of +// template overloads: the final frontier. +#ifndef _WIN32 +template <typename T, size_t N> +char (&ArraySizeHelper(const T (&array)[N]))[N]; +#endif + +#define arraysize(array) (sizeof(ArraySizeHelper(array))) + +// ARRAYSIZE performs essentially the same calculation as arraysize, +// but can be used on anonymous types or types defined inside +// functions. It's less safe than arraysize as it accepts some +// (although not all) pointers. Therefore, you should use arraysize +// whenever possible. +// +// The expression ARRAYSIZE(a) is a compile-time constant of type +// size_t. +// +// ARRAYSIZE catches a few type errors. If you see a compiler error +// +// "warning: division by zero in ..." +// +// when using ARRAYSIZE, you are (wrongfully) giving it a pointer. +// You should only use ARRAYSIZE on statically allocated arrays. +// +// The following comments are on the implementation details, and can +// be ignored by the users. +// +// ARRAYSIZE(arr) works by inspecting sizeof(arr) (the # of bytes in +// the array) and sizeof(*(arr)) (the # of bytes in one array +// element). If the former is divisible by the latter, perhaps arr is +// indeed an array, in which case the division result is the # of +// elements in the array. Otherwise, arr cannot possibly be an array, +// and we generate a compiler error to prevent the code from +// compiling. +// +// Since the size of bool is implementation-defined, we need to cast +// !(sizeof(a) & sizeof(*(a))) to size_t in order to ensure the final +// result has type size_t. +// +// This macro is not perfect as it wrongfully accepts certain +// pointers, namely where the pointer size is divisible by the pointee +// size. Since all our code has to go through a 32-bit compiler, +// where a pointer is 4 bytes, this means all pointers to a type whose +// size is 3 or greater than 4 will be (righteously) rejected. +// +// Kudos to Jorg Brown for this simple and elegant implementation. +// +// - wan 2005-11-16 +// +// Starting with Visual C++ 2005, WinNT.h includes ARRAYSIZE. However, +// the definition comes from the over-broad windows.h header that +// introduces a macro, ERROR, that conflicts with the logging framework +// that Ceres uses. Instead, rename ARRAYSIZE to CERES_ARRAYSIZE. +#define CERES_ARRAYSIZE(a) \ + ((sizeof(a) / sizeof(*(a))) / \ + static_cast<size_t>(!(sizeof(a) % sizeof(*(a))))) + +// Tell the compiler to warn about unused return values for functions declared +// with this macro. The macro should be used on function declarations +// following the argument list: +// +// Sprocket* AllocateSprocket() MUST_USE_RESULT; +// +#undef MUST_USE_RESULT +#if (__GNUC__ > 3 || (__GNUC__ == 3 && __GNUC_MINOR__ >= 4)) \ + && !defined(COMPILER_ICC) +#define MUST_USE_RESULT __attribute__ ((warn_unused_result)) +#else +#define MUST_USE_RESULT +#endif + +#endif // CERES_PUBLIC_INTERNAL_MACROS_H_ diff --git a/extern/libmv/third_party/ceres/include/ceres/internal/manual_constructor.h b/extern/libmv/third_party/ceres/include/ceres/internal/manual_constructor.h new file mode 100644 index 00000000000..a1d1f444e36 --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/internal/manual_constructor.h @@ -0,0 +1,214 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: kenton@google.com (Kenton Varda) +// +// ManualConstructor statically-allocates space in which to store some +// object, but does not initialize it. You can then call the constructor +// and destructor for the object yourself as you see fit. This is useful +// for memory management optimizations, where you want to initialize and +// destroy an object multiple times but only allocate it once. +// +// (When I say ManualConstructor statically allocates space, I mean that +// the ManualConstructor object itself is forced to be the right size.) + +#ifndef CERES_PUBLIC_INTERNAL_MANUAL_CONSTRUCTOR_H_ +#define CERES_PUBLIC_INTERNAL_MANUAL_CONSTRUCTOR_H_ + +#include <new> + +namespace ceres { +namespace internal { + +// ------- Define ALIGNED_CHAR_ARRAY -------------------------------- + +#ifndef ALIGNED_CHAR_ARRAY + +// Because MSVC and older GCCs require that the argument to their alignment +// construct to be a literal constant integer, we use a template instantiated +// at all the possible powers of two. +template<int alignment, int size> struct AlignType { }; +template<int size> struct AlignType<0, size> { typedef char result[size]; }; +#if defined(_MSC_VER) +#define BASE_PORT_H_ALIGN_ATTRIBUTE(X) __declspec(align(X)) +#define BASE_PORT_H_ALIGN_OF(T) __alignof(T) +#elif defined(__GNUC__) +#define BASE_PORT_H_ALIGN_ATTRIBUTE(X) __attribute__((aligned(X))) +#define BASE_PORT_H_ALIGN_OF(T) __alignof__(T) +#endif + +#if defined(BASE_PORT_H_ALIGN_ATTRIBUTE) + +#define BASE_PORT_H_ALIGNTYPE_TEMPLATE(X) \ + template<int size> struct AlignType<X, size> { \ + typedef BASE_PORT_H_ALIGN_ATTRIBUTE(X) char result[size]; \ + } + +BASE_PORT_H_ALIGNTYPE_TEMPLATE(1); +BASE_PORT_H_ALIGNTYPE_TEMPLATE(2); +BASE_PORT_H_ALIGNTYPE_TEMPLATE(4); +BASE_PORT_H_ALIGNTYPE_TEMPLATE(8); +BASE_PORT_H_ALIGNTYPE_TEMPLATE(16); +BASE_PORT_H_ALIGNTYPE_TEMPLATE(32); +BASE_PORT_H_ALIGNTYPE_TEMPLATE(64); +BASE_PORT_H_ALIGNTYPE_TEMPLATE(128); +BASE_PORT_H_ALIGNTYPE_TEMPLATE(256); +BASE_PORT_H_ALIGNTYPE_TEMPLATE(512); +BASE_PORT_H_ALIGNTYPE_TEMPLATE(1024); +BASE_PORT_H_ALIGNTYPE_TEMPLATE(2048); +BASE_PORT_H_ALIGNTYPE_TEMPLATE(4096); +BASE_PORT_H_ALIGNTYPE_TEMPLATE(8192); +// Any larger and MSVC++ will complain. + +#define ALIGNED_CHAR_ARRAY(T, Size) \ + typename AlignType<BASE_PORT_H_ALIGN_OF(T), sizeof(T) * Size>::result + +#undef BASE_PORT_H_ALIGNTYPE_TEMPLATE +#undef BASE_PORT_H_ALIGN_ATTRIBUTE + +#else // defined(BASE_PORT_H_ALIGN_ATTRIBUTE) +#define ALIGNED_CHAR_ARRAY you_must_define_ALIGNED_CHAR_ARRAY_for_your_compiler +#endif // defined(BASE_PORT_H_ALIGN_ATTRIBUTE) + +#undef BASE_PORT_H_ALIGNTYPE_TEMPLATE +#undef BASE_PORT_H_ALIGN_ATTRIBUTE + +#endif // ALIGNED_CHAR_ARRAY + +template <typename Type> +class ManualConstructor { + public: + // No constructor or destructor because one of the most useful uses of + // this class is as part of a union, and members of a union cannot have + // constructors or destructors. And, anyway, the whole point of this + // class is to bypass these. + + inline Type* get() { + return reinterpret_cast<Type*>(space_); + } + inline const Type* get() const { + return reinterpret_cast<const Type*>(space_); + } + + inline Type* operator->() { return get(); } + inline const Type* operator->() const { return get(); } + + inline Type& operator*() { return *get(); } + inline const Type& operator*() const { return *get(); } + + // You can pass up to four constructor arguments as arguments of Init(). + inline void Init() { + new(space_) Type; + } + + template <typename T1> + inline void Init(const T1& p1) { + new(space_) Type(p1); + } + + template <typename T1, typename T2> + inline void Init(const T1& p1, const T2& p2) { + new(space_) Type(p1, p2); + } + + template <typename T1, typename T2, typename T3> + inline void Init(const T1& p1, const T2& p2, const T3& p3) { + new(space_) Type(p1, p2, p3); + } + + template <typename T1, typename T2, typename T3, typename T4> + inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4) { + new(space_) Type(p1, p2, p3, p4); + } + + template <typename T1, typename T2, typename T3, typename T4, typename T5> + inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4, + const T5& p5) { + new(space_) Type(p1, p2, p3, p4, p5); + } + + template <typename T1, typename T2, typename T3, typename T4, typename T5, + typename T6> + inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4, + const T5& p5, const T6& p6) { + new(space_) Type(p1, p2, p3, p4, p5, p6); + } + + template <typename T1, typename T2, typename T3, typename T4, typename T5, + typename T6, typename T7> + inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4, + const T5& p5, const T6& p6, const T7& p7) { + new(space_) Type(p1, p2, p3, p4, p5, p6, p7); + } + + template <typename T1, typename T2, typename T3, typename T4, typename T5, + typename T6, typename T7, typename T8> + inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4, + const T5& p5, const T6& p6, const T7& p7, const T8& p8) { + new(space_) Type(p1, p2, p3, p4, p5, p6, p7, p8); + } + + template <typename T1, typename T2, typename T3, typename T4, typename T5, + typename T6, typename T7, typename T8, typename T9> + inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4, + const T5& p5, const T6& p6, const T7& p7, const T8& p8, + const T9& p9) { + new(space_) Type(p1, p2, p3, p4, p5, p6, p7, p8, p9); + } + + template <typename T1, typename T2, typename T3, typename T4, typename T5, + typename T6, typename T7, typename T8, typename T9, typename T10> + inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4, + const T5& p5, const T6& p6, const T7& p7, const T8& p8, + const T9& p9, const T10& p10) { + new(space_) Type(p1, p2, p3, p4, p5, p6, p7, p8, p9, p10); + } + + template <typename T1, typename T2, typename T3, typename T4, typename T5, + typename T6, typename T7, typename T8, typename T9, typename T10, + typename T11> + inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4, + const T5& p5, const T6& p6, const T7& p7, const T8& p8, + const T9& p9, const T10& p10, const T11& p11) { + new(space_) Type(p1, p2, p3, p4, p5, p6, p7, p8, p9, p10, p11); + } + + inline void Destroy() { + get()->~Type(); + } + + private: + ALIGNED_CHAR_ARRAY(Type, 1) space_; +}; + +#undef ALIGNED_CHAR_ARRAY + +} // namespace internal +} // namespace ceres + +#endif // CERES_PUBLIC_INTERNAL_MANUAL_CONSTRUCTOR_H_ diff --git a/extern/libmv/third_party/ceres/include/ceres/internal/port.h b/extern/libmv/third_party/ceres/include/ceres/internal/port.h new file mode 100644 index 00000000000..9a3e5cced58 --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/internal/port.h @@ -0,0 +1,44 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#ifndef CERES_PUBLIC_INTERNAL_PORT_H_ +#define CERES_PUBLIC_INTERNAL_PORT_H_ + +namespace ceres { + +// It is unfortunate that this import of the entire standard namespace is +// necessary. The reasons are historical and won't be explained here, but +// suffice to say it is not a mistake and can't be removed without breaking +// things outside of the Ceres optimization package. +using namespace std; + +} // namespace ceres + +#endif // CERES_PUBLIC_INTERNAL_PORT_H_ diff --git a/extern/libmv/third_party/ceres/include/ceres/internal/scoped_ptr.h b/extern/libmv/third_party/ceres/include/ceres/internal/scoped_ptr.h new file mode 100644 index 00000000000..44f198b339d --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/internal/scoped_ptr.h @@ -0,0 +1,311 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: jorg@google.com (Jorg Brown) +// +// This is an implementation designed to match the anticipated future TR2 +// implementation of the scoped_ptr class, and its closely-related brethren, +// scoped_array, scoped_ptr_malloc, and make_scoped_ptr. + +#ifndef CERES_PUBLIC_INTERNAL_SCOPED_PTR_H_ +#define CERES_PUBLIC_INTERNAL_SCOPED_PTR_H_ + +#include <assert.h> +#include <stdlib.h> +#include <cstddef> + +namespace ceres { +namespace internal { + +template <class C> class scoped_ptr; +template <class C, class Free> class scoped_ptr_malloc; +template <class C> class scoped_array; + +template <class C> +scoped_ptr<C> make_scoped_ptr(C *); + +// A scoped_ptr<T> is like a T*, except that the destructor of scoped_ptr<T> +// automatically deletes the pointer it holds (if any). That is, scoped_ptr<T> +// owns the T object that it points to. Like a T*, a scoped_ptr<T> may hold +// either NULL or a pointer to a T object. Also like T*, scoped_ptr<T> is +// thread-compatible, and once you dereference it, you get the threadsafety +// guarantees of T. +// +// The size of a scoped_ptr is small: sizeof(scoped_ptr<C>) == sizeof(C*) +template <class C> +class scoped_ptr { + public: + + // The element type + typedef C element_type; + + // Constructor. Defaults to intializing with NULL. + // There is no way to create an uninitialized scoped_ptr. + // The input parameter must be allocated with new. + explicit scoped_ptr(C* p = NULL) : ptr_(p) { } + + // Destructor. If there is a C object, delete it. + // We don't need to test ptr_ == NULL because C++ does that for us. + ~scoped_ptr() { + enum { type_must_be_complete = sizeof(C) }; + delete ptr_; + } + + // Reset. Deletes the current owned object, if any. + // Then takes ownership of a new object, if given. + // this->reset(this->get()) works. + void reset(C* p = NULL) { + if (p != ptr_) { + enum { type_must_be_complete = sizeof(C) }; + delete ptr_; + ptr_ = p; + } + } + + // Accessors to get the owned object. + // operator* and operator-> will assert() if there is no current object. + C& operator*() const { + assert(ptr_ != NULL); + return *ptr_; + } + C* operator->() const { + assert(ptr_ != NULL); + return ptr_; + } + C* get() const { return ptr_; } + + // Comparison operators. + // These return whether a scoped_ptr and a raw pointer refer to + // the same object, not just to two different but equal objects. + bool operator==(const C* p) const { return ptr_ == p; } + bool operator!=(const C* p) const { return ptr_ != p; } + + // Swap two scoped pointers. + void swap(scoped_ptr& p2) { + C* tmp = ptr_; + ptr_ = p2.ptr_; + p2.ptr_ = tmp; + } + + // Release a pointer. + // The return value is the current pointer held by this object. + // If this object holds a NULL pointer, the return value is NULL. + // After this operation, this object will hold a NULL pointer, + // and will not own the object any more. + C* release() { + C* retVal = ptr_; + ptr_ = NULL; + return retVal; + } + + private: + C* ptr_; + + // google3 friend class that can access copy ctor (although if it actually + // calls a copy ctor, there will be a problem) see below + friend scoped_ptr<C> make_scoped_ptr<C>(C *p); + + // Forbid comparison of scoped_ptr types. If C2 != C, it totally doesn't + // make sense, and if C2 == C, it still doesn't make sense because you should + // never have the same object owned by two different scoped_ptrs. + template <class C2> bool operator==(scoped_ptr<C2> const& p2) const; + template <class C2> bool operator!=(scoped_ptr<C2> const& p2) const; + + // Disallow evil constructors + scoped_ptr(const scoped_ptr&); + void operator=(const scoped_ptr&); +}; + +// Free functions +template <class C> +inline void swap(scoped_ptr<C>& p1, scoped_ptr<C>& p2) { + p1.swap(p2); +} + +template <class C> +inline bool operator==(const C* p1, const scoped_ptr<C>& p2) { + return p1 == p2.get(); +} + +template <class C> +inline bool operator==(const C* p1, const scoped_ptr<const C>& p2) { + return p1 == p2.get(); +} + +template <class C> +inline bool operator!=(const C* p1, const scoped_ptr<C>& p2) { + return p1 != p2.get(); +} + +template <class C> +inline bool operator!=(const C* p1, const scoped_ptr<const C>& p2) { + return p1 != p2.get(); +} + +template <class C> +scoped_ptr<C> make_scoped_ptr(C *p) { + // This does nothing but to return a scoped_ptr of the type that the passed + // pointer is of. (This eliminates the need to specify the name of T when + // making a scoped_ptr that is used anonymously/temporarily.) From an + // access control point of view, we construct an unnamed scoped_ptr here + // which we return and thus copy-construct. Hence, we need to have access + // to scoped_ptr::scoped_ptr(scoped_ptr const &). However, it is guaranteed + // that we never actually call the copy constructor, which is a good thing + // as we would call the temporary's object destructor (and thus delete p) + // if we actually did copy some object, here. + return scoped_ptr<C>(p); +} + +// scoped_array<C> is like scoped_ptr<C>, except that the caller must allocate +// with new [] and the destructor deletes objects with delete []. +// +// As with scoped_ptr<C>, a scoped_array<C> either points to an object +// or is NULL. A scoped_array<C> owns the object that it points to. +// scoped_array<T> is thread-compatible, and once you index into it, +// the returned objects have only the threadsafety guarantees of T. +// +// Size: sizeof(scoped_array<C>) == sizeof(C*) +template <class C> +class scoped_array { + public: + + // The element type + typedef C element_type; + + // Constructor. Defaults to intializing with NULL. + // There is no way to create an uninitialized scoped_array. + // The input parameter must be allocated with new []. + explicit scoped_array(C* p = NULL) : array_(p) { } + + // Destructor. If there is a C object, delete it. + // We don't need to test ptr_ == NULL because C++ does that for us. + ~scoped_array() { + enum { type_must_be_complete = sizeof(C) }; + delete[] array_; + } + + // Reset. Deletes the current owned object, if any. + // Then takes ownership of a new object, if given. + // this->reset(this->get()) works. + void reset(C* p = NULL) { + if (p != array_) { + enum { type_must_be_complete = sizeof(C) }; + delete[] array_; + array_ = p; + } + } + + // Get one element of the current object. + // Will assert() if there is no current object, or index i is negative. + C& operator[](std::ptrdiff_t i) const { + assert(i >= 0); + assert(array_ != NULL); + return array_[i]; + } + + // Get a pointer to the zeroth element of the current object. + // If there is no current object, return NULL. + C* get() const { + return array_; + } + + // Comparison operators. + // These return whether a scoped_array and a raw pointer refer to + // the same array, not just to two different but equal arrays. + bool operator==(const C* p) const { return array_ == p; } + bool operator!=(const C* p) const { return array_ != p; } + + // Swap two scoped arrays. + void swap(scoped_array& p2) { + C* tmp = array_; + array_ = p2.array_; + p2.array_ = tmp; + } + + // Release an array. + // The return value is the current pointer held by this object. + // If this object holds a NULL pointer, the return value is NULL. + // After this operation, this object will hold a NULL pointer, + // and will not own the object any more. + C* release() { + C* retVal = array_; + array_ = NULL; + return retVal; + } + + private: + C* array_; + + // Forbid comparison of different scoped_array types. + template <class C2> bool operator==(scoped_array<C2> const& p2) const; + template <class C2> bool operator!=(scoped_array<C2> const& p2) const; + + // Disallow evil constructors + scoped_array(const scoped_array&); + void operator=(const scoped_array&); +}; + +// Free functions +template <class C> +inline void swap(scoped_array<C>& p1, scoped_array<C>& p2) { + p1.swap(p2); +} + +template <class C> +inline bool operator==(const C* p1, const scoped_array<C>& p2) { + return p1 == p2.get(); +} + +template <class C> +inline bool operator==(const C* p1, const scoped_array<const C>& p2) { + return p1 == p2.get(); +} + +template <class C> +inline bool operator!=(const C* p1, const scoped_array<C>& p2) { + return p1 != p2.get(); +} + +template <class C> +inline bool operator!=(const C* p1, const scoped_array<const C>& p2) { + return p1 != p2.get(); +} + +// This class wraps the c library function free() in a class that can be +// passed as a template argument to scoped_ptr_malloc below. +class ScopedPtrMallocFree { + public: + inline void operator()(void* x) const { + free(x); + } +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_PUBLIC_INTERNAL_SCOPED_PTR_H_ diff --git a/extern/libmv/third_party/ceres/include/ceres/iteration_callback.h b/extern/libmv/third_party/ceres/include/ceres/iteration_callback.h new file mode 100644 index 00000000000..88da992d0c5 --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/iteration_callback.h @@ -0,0 +1,159 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// When an iteration callback is specified, Ceres calls the callback after each +// optimizer step and pass it an IterationSummary object, defined below. + +#ifndef CERES_PUBLIC_ITERATION_CALLBACK_H_ +#define CERES_PUBLIC_ITERATION_CALLBACK_H_ + +#include "ceres/types.h" + +namespace ceres { + +// This struct describes the state of the optimizer after each +// iteration of the minimization. +struct IterationSummary { + // Current iteration number. + int32 iteration; + + // Whether or not the algorithm made progress in this iteration. + bool step_is_successful; + + // Value of the objective function. + double cost; + + // Change in the value of the objective function in this + // iteration. This can be positive or negative. Negative change + // means that the step was not successful. + double cost_change; + + // Infinity norm of the gradient vector. + double gradient_max_norm; + + // 2-norm of the size of the step computed by the optimization + // algorithm. + double step_norm; + + // For trust region algorithms, the ratio of the actual change in + // cost and the change in the cost of the linearized approximation. + double relative_decrease; + + // Value of the regularization parameter for Levenberg-Marquardt + // algorithm at the end of the current iteration. + double mu; + + // For the inexact step Levenberg-Marquardt algorithm, this is the + // relative accuracy with which the Newton(LM) step is solved. This + // number affects only the iterative solvers capable of solving + // linear systems inexactly. Factorization-based exact solvers + // ignore it. + double eta; + + // Number of iterations taken by the linear solver to solve for the + // Newton step. + int linear_solver_iterations; + + // TODO(sameeragarwal): Change to use a higher precision timer using + // clock_gettime. + // Time (in seconds) spent inside the linear least squares solver. + int iteration_time_sec; + + // Time (in seconds) spent inside the linear least squares solver. + int linear_solver_time_sec; +}; + +// Interface for specifying callbacks that are executed at the end of +// each iteration of the Minimizer. The solver uses the return value +// of operator() to decide whether to continue solving or to +// terminate. The user can return three values. +// +// SOLVER_ABORT indicates that the callback detected an abnormal +// situation. The solver returns without updating the parameter blocks +// (unless Solver::Options::update_state_every_iteration is set +// true). Solver returns with Solver::Summary::termination_type set to +// USER_ABORT. +// +// SOLVER_TERMINATE_SUCCESSFULLY indicates that there is no need to +// optimize anymore (some user specified termination criterion has +// been met). Solver returns with Solver::Summary::termination_type +// set to USER_SUCCESS. +// +// SOLVER_CONTINUE indicates that the solver should continue +// optimizing. +// +// For example, the following Callback is used internally by Ceres to +// log the progress of the optimization. +// +// Callback for logging the state of the minimizer to STDERR or STDOUT +// depending on the user's preferences and logging level. +// +// class LoggingCallback : public IterationCallback { +// public: +// explicit LoggingCallback(bool log_to_stdout) +// : log_to_stdout_(log_to_stdout) {} +// +// ~LoggingCallback() {} +// +// CallbackReturnType operator()(const IterationSummary& summary) { +// const char* kReportRowFormat = +// "% 4d: f:% 8e d:% 3.2e g:% 3.2e h:% 3.2e " +// "rho:% 3.2e mu:% 3.2e eta:% 3.2e li:% 3d"; +// string output = StringPrintf(kReportRowFormat, +// summary.iteration, +// summary.cost, +// summary.cost_change, +// summary.gradient_max_norm, +// summary.step_norm, +// summary.relative_decrease, +// summary.mu, +// summary.eta, +// summary.linear_solver_iterations); +// if (log_to_stdout_) { +// cout << output << endl; +// } else { +// VLOG(1) << output; +// } +// return SOLVER_CONTINUE; +// } +// +// private: +// const bool log_to_stdout_; +// }; +// +class IterationCallback { + public: + virtual ~IterationCallback() {} + virtual CallbackReturnType operator()(const IterationSummary& summary) = 0; +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_ITERATION_CALLBACK_H_ diff --git a/extern/libmv/third_party/ceres/include/ceres/jet.h b/extern/libmv/third_party/ceres/include/ceres/jet.h new file mode 100644 index 00000000000..264861735ed --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/jet.h @@ -0,0 +1,755 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// A simple implementation of N-dimensional dual numbers, for automatically +// computing exact derivatives of functions. +// +// While a complete treatment of the mechanics of automatic differentation is +// beyond the scope of this header (see +// http://en.wikipedia.org/wiki/Automatic_differentiation for details), the +// basic idea is to extend normal arithmetic with an extra element, "e," often +// denoted with the greek symbol epsilon, such that e != 0 but e^2 = 0. Dual +// numbers are extensions of the real numbers analogous to complex numbers: +// whereas complex numbers augment the reals by introducing an imaginary unit i +// such that i^2 = -1, dual numbers introduce an "infinitesimal" unit e such +// that e^2 = 0. Dual numbers have two components: the "real" component and the +// "infinitesimal" component, generally written as x + y*e. Surprisingly, this +// leads to a convenient method for computing exact derivatives without needing +// to manipulate complicated symbolic expressions. +// +// For example, consider the function +// +// f(x) = x^2 , +// +// evaluated at 10. Using normal arithmetic, f(10) = 100, and df/dx(10) = 20. +// Next, augument 10 with an infinitesimal to get: +// +// f(10 + e) = (10 + e)^2 +// = 100 + 2 * 10 * e + e^2 +// = 100 + 20 * e -+- +// -- | +// | +--- This is zero, since e^2 = 0 +// | +// +----------------- This is df/dx! +// +// Note that the derivative of f with respect to x is simply the infinitesimal +// component of the value of f(x + e). So, in order to take the derivative of +// any function, it is only necessary to replace the numeric "object" used in +// the function with one extended with infinitesimals. The class Jet, defined in +// this header, is one such example of this, where substitution is done with +// templates. +// +// To handle derivatives of functions taking multiple arguments, different +// infinitesimals are used, one for each variable to take the derivative of. For +// example, consider a scalar function of two scalar parameters x and y: +// +// f(x, y) = x^2 + x * y +// +// Following the technique above, to compute the derivatives df/dx and df/dy for +// f(1, 3) involves doing two evaluations of f, the first time replacing x with +// x + e, the second time replacing y with y + e. +// +// For df/dx: +// +// f(1 + e, y) = (1 + e)^2 + (1 + e) * 3 +// = 1 + 2 * e + 3 + 3 * e +// = 4 + 5 * e +// +// --> df/dx = 5 +// +// For df/dy: +// +// f(1, 3 + e) = 1^2 + 1 * (3 + e) +// = 1 + 3 + e +// = 4 + e +// +// --> df/dy = 1 +// +// To take the gradient of f with the implementation of dual numbers ("jets") in +// this file, it is necessary to create a single jet type which has components +// for the derivative in x and y, and passing them to a templated version of f: +// +// template<typename T> +// T f(const T &x, const T &y) { +// return x * x + x * y; +// } +// +// // The "2" means there should be 2 dual number components. +// Jet<double, 2> x(0); // Pick the 0th dual number for x. +// Jet<double, 2> y(1); // Pick the 1st dual number for y. +// Jet<double, 2> z = f(x, y); +// +// LG << "df/dx = " << z.a[0] +// << "df/dy = " << z.a[1]; +// +// Most users should not use Jet objects directly; a wrapper around Jet objects, +// which makes computing the derivative, gradient, or jacobian of templated +// functors simple, is in autodiff.h. Even autodiff.h should not be used +// directly; instead autodiff_cost_function.h is typically the file of interest. +// +// For the more mathematically inclined, this file implements first-order +// "jets". A 1st order jet is an element of the ring +// +// T[N] = T[t_1, ..., t_N] / (t_1, ..., t_N)^2 +// +// which essentially means that each jet consists of a "scalar" value 'a' from T +// and a 1st order perturbation vector 'v' of length N: +// +// x = a + \sum_i v[i] t_i +// +// A shorthand is to write an element as x = a + u, where u is the pertubation. +// Then, the main point about the arithmetic of jets is that the product of +// perturbations is zero: +// +// (a + u) * (b + v) = ab + av + bu + uv +// = ab + (av + bu) + 0 +// +// which is what operator* implements below. Addition is simpler: +// +// (a + u) + (b + v) = (a + b) + (u + v). +// +// The only remaining question is how to evaluate the function of a jet, for +// which we use the chain rule: +// +// f(a + u) = f(a) + f'(a) u +// +// where f'(a) is the (scalar) derivative of f at a. +// +// By pushing these things through sufficiently and suitably templated +// functions, we can do automatic differentiation. Just be sure to turn on +// function inlining and common-subexpression elimination, or it will be very +// slow! +// +// WARNING: Most Ceres users should not directly include this file or know the +// details of how jets work. Instead the suggested method for automatic +// derivatives is to use autodiff_cost_function.h, which is a wrapper around +// both jets.h and autodiff.h to make taking derivatives of cost functions for +// use in Ceres easier. + +#ifndef CERES_PUBLIC_JET_H_ +#define CERES_PUBLIC_JET_H_ + +#include <cmath> +#include <iosfwd> +#include <iostream> // NOLINT +#include <string> + +#include "Eigen/Core" + +// Visual Studio 2010 or older version +#if defined(_MSC_VER) && _MSC_VER <= 1600 +namespace std { +inline bool isfinite(double x) { return _finite(x); } +inline bool isinf (double x) { return !_finite(x) && !_isnan(x); } +inline bool isnan (double x) { return _isnan(x); } +inline bool isnormal(double x) { return _finite(x) && x != 0.0; } +} // namespace std +#endif + +namespace ceres { + +template <typename T, int N> +struct Jet { + enum { DIMENSION = N }; + + // Default-construct "a" because otherwise this can lead to false errors about + // uninitialized uses when other classes relying on default constructed T + // (where T is a Jet<T, N>). This usually only happens in opt mode. Note that + // the C++ standard mandates that e.g. default constructed doubles are + // initialized to 0.0; see sections 8.5 of the C++03 standard. + Jet() : a() {} + + // Constructor from scalar: a + 0. + explicit Jet(const T& value) { + a = value; + v.setZero(); + } + + // Constructor from scalar plus variable: a + t_i. + Jet(const T& value, int k) { + a = value; + v.setZero(); + v[k] = T(1.0); + } + + /* + + // Construct from an array where the first element is the scalar. + // This is templated to support converting from other data types. + template<typename D> + Jet(const D* scalar_and_derivatives) { + a = T(scalar_and_derivatives[0]); + v = Eigen::Map<const Eigen::Matrix<D, N, 1> >( + scalar_and_derivatives + 1, N).cast<T>(); + } + */ + + // Compound operators + Jet<T, N>& operator+=(const Jet<T, N> &y) { + *this = *this + y; + return *this; + } + + Jet<T, N>& operator-=(const Jet<T, N> &y) { + *this = *this - y; + return *this; + } + + Jet<T, N>& operator*=(const Jet<T, N> &y) { + *this = *this * y; + return *this; + } + + Jet<T, N>& operator/=(const Jet<T, N> &y) { + *this = *this / y; + return *this; + } + + T a; // The scalar part. + Eigen::Matrix<T, N, 1> v; // The infinitesimal part. +}; + +// Unary + +template<typename T, int N> inline +Jet<T, N> const& operator+(const Jet<T, N>& f) { + return f; +} + +// TODO(keir): Try adding __attribute__((always_inline)) to these functions to +// see if it causes a performance increase. + +// Unary - +template<typename T, int N> inline +Jet<T, N> operator-(const Jet<T, N>&f) { + Jet<T, N> g; + g.a = -f.a; + g.v = -f.v; + return g; +} + +// Binary + +template<typename T, int N> inline +Jet<T, N> operator+(const Jet<T, N>& f, + const Jet<T, N>& g) { + Jet<T, N> h; + h.a = f.a + g.a; + h.v = f.v + g.v; + return h; +} + +// Binary + with a scalar: x + s +template<typename T, int N> inline +Jet<T, N> operator+(const Jet<T, N>& f, T s) { + Jet<T, N> h; + h.a = f.a + s; + h.v = f.v; + return h; +} + +// Binary + with a scalar: s + x +template<typename T, int N> inline +Jet<T, N> operator+(T s, const Jet<T, N>& f) { + Jet<T, N> h; + h.a = f.a + s; + h.v = f.v; + return h; +} + +// Binary - +template<typename T, int N> inline +Jet<T, N> operator-(const Jet<T, N>& f, + const Jet<T, N>& g) { + Jet<T, N> h; + h.a = f.a - g.a; + h.v = f.v - g.v; + return h; +} + +// Binary - with a scalar: x - s +template<typename T, int N> inline +Jet<T, N> operator-(const Jet<T, N>& f, T s) { + Jet<T, N> h; + h.a = f.a - s; + h.v = f.v; + return h; +} + +// Binary - with a scalar: s - x +template<typename T, int N> inline +Jet<T, N> operator-(T s, const Jet<T, N>& f) { + Jet<T, N> h; + h.a = s - f.a; + h.v = -f.v; + return h; +} + +// Binary * +template<typename T, int N> inline +Jet<T, N> operator*(const Jet<T, N>& f, + const Jet<T, N>& g) { + Jet<T, N> h; + h.a = f.a * g.a; + h.v = f.a * g.v + f.v * g.a; + return h; +} + +// Binary * with a scalar: x * s +template<typename T, int N> inline +Jet<T, N> operator*(const Jet<T, N>& f, T s) { + Jet<T, N> h; + h.a = f.a * s; + h.v = f.v * s; + return h; +} + +// Binary * with a scalar: s * x +template<typename T, int N> inline +Jet<T, N> operator*(T s, const Jet<T, N>& f) { + Jet<T, N> h; + h.a = f.a * s; + h.v = f.v * s; + return h; +} + +// Binary / +template<typename T, int N> inline +Jet<T, N> operator/(const Jet<T, N>& f, + const Jet<T, N>& g) { + Jet<T, N> h; + // This uses: + // + // a + u (a + u)(b - v) (a + u)(b - v) + // ----- = -------------- = -------------- + // b + v (b + v)(b - v) b^2 + // + // which holds because v*v = 0. + h.a = f.a / g.a; + h.v = (f.v - f.a / g.a * g.v) / g.a; + return h; +} + +// Binary / with a scalar: s / x +template<typename T, int N> inline +Jet<T, N> operator/(T s, const Jet<T, N>& g) { + Jet<T, N> h; + h.a = s / g.a; + h.v = - s * g.v / (g.a * g.a); + return h; +} + +// Binary / with a scalar: x / s +template<typename T, int N> inline +Jet<T, N> operator/(const Jet<T, N>& f, T s) { + Jet<T, N> h; + h.a = f.a / s; + h.v = f.v / s; + return h; +} + +// Binary comparison operators for both scalars and jets. +#define CERES_DEFINE_JET_COMPARISON_OPERATOR(op) \ +template<typename T, int N> inline \ +bool operator op(const Jet<T, N>& f, const Jet<T, N>& g) { \ + return f.a op g.a; \ +} \ +template<typename T, int N> inline \ +bool operator op(const T& s, const Jet<T, N>& g) { \ + return s op g.a; \ +} \ +template<typename T, int N> inline \ +bool operator op(const Jet<T, N>& f, const T& s) { \ + return f.a op s; \ +} +CERES_DEFINE_JET_COMPARISON_OPERATOR( < ) // NOLINT +CERES_DEFINE_JET_COMPARISON_OPERATOR( <= ) // NOLINT +CERES_DEFINE_JET_COMPARISON_OPERATOR( > ) // NOLINT +CERES_DEFINE_JET_COMPARISON_OPERATOR( >= ) // NOLINT +CERES_DEFINE_JET_COMPARISON_OPERATOR( == ) // NOLINT +CERES_DEFINE_JET_COMPARISON_OPERATOR( != ) // NOLINT +#undef CERES_DEFINE_JET_COMPARISON_OPERATOR + +// Pull some functions from namespace std. +// +// This is necessary because we want to use the same name (e.g. 'sqrt') for +// double-valued and Jet-valued functions, but we are not allowed to put +// Jet-valued functions inside namespace std. +// +// Missing: cosh, sinh, tanh, tan +// TODO(keir): Switch to "using". +inline double abs (double x) { return std::abs(x); } +inline double log (double x) { return std::log(x); } +inline double exp (double x) { return std::exp(x); } +inline double sqrt (double x) { return std::sqrt(x); } +inline double cos (double x) { return std::cos(x); } +inline double acos (double x) { return std::acos(x); } +inline double sin (double x) { return std::sin(x); } +inline double asin (double x) { return std::asin(x); } +inline bool isfinite(double x) { return std::isfinite(x); } +inline bool isinf (double x) { return std::isinf(x); } +inline bool isnan (double x) { return std::isnan(x); } +inline bool isnormal(double x) { return std::isnormal(x); } +inline double pow (double x, double y) { return std::pow(x, y); } +inline double atan2(double y, double x) { return std::atan2(y, x); } + +// In general, f(a + h) ~= f(a) + f'(a) h, via the chain rule. + +// abs(x + h) ~= x + h or -(x + h) +template <typename T, int N> inline +Jet<T, N> abs(const Jet<T, N>& f) { + return f.a < T(0.0) ? -f : f; +} + +// log(a + h) ~= log(a) + h / a +template <typename T, int N> inline +Jet<T, N> log(const Jet<T, N>& f) { + Jet<T, N> g; + g.a = log(f.a); + g.v = f.v / f.a; + return g; +} + +// exp(a + h) ~= exp(a) + exp(a) h +template <typename T, int N> inline +Jet<T, N> exp(const Jet<T, N>& f) { + Jet<T, N> g; + g.a = exp(f.a); + g.v = g.a * f.v; + return g; +} + +// sqrt(a + h) ~= sqrt(a) + h / (2 sqrt(a)) +template <typename T, int N> inline +Jet<T, N> sqrt(const Jet<T, N>& f) { + Jet<T, N> g; + g.a = sqrt(f.a); + g.v = f.v / (T(2.0) * g.a); + return g; +} + +// cos(a + h) ~= cos(a) - sin(a) h +template <typename T, int N> inline +Jet<T, N> cos(const Jet<T, N>& f) { + Jet<T, N> g; + g.a = cos(f.a); + T sin_a = sin(f.a); + g.v = - sin_a * f.v; + return g; +} + +// acos(a + h) ~= acos(a) - 1 / sqrt(1 - a^2) h +template <typename T, int N> inline +Jet<T, N> acos(const Jet<T, N>& f) { + Jet<T, N> g; + g.a = acos(f.a); + g.v = - T(1.0) / sqrt(T(1.0) - f.a * f.a) * f.v; + return g; +} + +// sin(a + h) ~= sin(a) + cos(a) h +template <typename T, int N> inline +Jet<T, N> sin(const Jet<T, N>& f) { + Jet<T, N> g; + g.a = sin(f.a); + T cos_a = cos(f.a); + g.v = cos_a * f.v; + return g; +} + +// asin(a + h) ~= asin(a) + 1 / sqrt(1 - a^2) h +template <typename T, int N> inline +Jet<T, N> asin(const Jet<T, N>& f) { + Jet<T, N> g; + g.a = asin(f.a); + g.v = T(1.0) / sqrt(T(1.0) - f.a * f.a) * f.v; + return g; +} + +// Jet Classification. It is not clear what the appropriate semantics are for +// these classifications. This picks that isfinite and isnormal are "all" +// operations, i.e. all elements of the jet must be finite for the jet itself to +// be finite (or normal). For isnan and isinf, the answer is less clear. This +// takes a "any" approach for isnan and isinf such that if any part of a jet is +// nan or inf, then the entire jet is nan or inf. This leads to strange +// situations like a jet can be both isinf and isnan, but in practice the "any" +// semantics are the most useful for e.g. checking that derivatives are sane. + +// The jet is finite if all parts of the jet are finite. +template <typename T, int N> inline +bool isfinite(const Jet<T, N>& f) { + if (!isfinite(f.a)) { + return false; + } + for (int i = 0; i < N; ++i) { + if (!isfinite(f.v[i])) { + return false; + } + } + return true; +} + +// The jet is infinite if any part of the jet is infinite. +template <typename T, int N> inline +bool isinf(const Jet<T, N>& f) { + if (isinf(f.a)) { + return true; + } + for (int i = 0; i < N; i++) { + if (isinf(f.v[i])) { + return true; + } + } + return false; +} + +// The jet is NaN if any part of the jet is NaN. +template <typename T, int N> inline +bool isnan(const Jet<T, N>& f) { + if (isnan(f.a)) { + return true; + } + for (int i = 0; i < N; ++i) { + if (isnan(f.v[i])) { + return true; + } + } + return false; +} + +// The jet is normal if all parts of the jet are normal. +template <typename T, int N> inline +bool isnormal(const Jet<T, N>& f) { + if (!isnormal(f.a)) { + return false; + } + for (int i = 0; i < N; ++i) { + if (!isnormal(f.v[i])) { + return false; + } + } + return true; +} + +// atan2(b + db, a + da) ~= atan2(b, a) + (- b da + a db) / (a^2 + b^2) +// +// In words: the rate of change of theta is 1/r times the rate of +// change of (x, y) in the positive angular direction. +template <typename T, int N> inline +Jet<T, N> atan2(const Jet<T, N>& g, const Jet<T, N>& f) { + // Note order of arguments: + // + // f = a + da + // g = b + db + + Jet<T, N> out; + + out.a = atan2(g.a, f.a); + + T const temp = T(1.0) / (f.a * f.a + g.a * g.a); + out.v = temp * (- g.a * f.v + f.a * g.v); + return out; +} + + +// pow -- base is a differentiatble function, exponent is a constant. +// (a+da)^p ~= a^p + p*a^(p-1) da +template <typename T, int N> inline +Jet<T, N> pow(const Jet<T, N>& f, double g) { + Jet<T, N> out; + out.a = pow(f.a, g); + T const temp = g * pow(f.a, g - T(1.0)); + out.v = temp * f.v; + return out; +} + +// pow -- base is a constant, exponent is a differentiable function. +// (a)^(p+dp) ~= a^p + a^p log(a) dp +template <typename T, int N> inline +Jet<T, N> pow(double f, const Jet<T, N>& g) { + Jet<T, N> out; + out.a = pow(f, g.a); + T const temp = log(f) * out.a; + out.v = temp * g.v; + return out; +} + + +// pow -- both base and exponent are differentiable functions. +// (a+da)^(b+db) ~= a^b + b * a^(b-1) da + a^b log(a) * db +template <typename T, int N> inline +Jet<T, N> pow(const Jet<T, N>& f, const Jet<T, N>& g) { + Jet<T, N> out; + + T const temp1 = pow(f.a, g.a); + T const temp2 = g.a * pow(f.a, g.a - T(1.0)); + T const temp3 = temp1 * log(f.a); + + out.a = temp1; + out.v = temp2 * f.v + temp3 * g.v; + return out; +} + +// Define the helper functions Eigen needs to embed Jet types. +// +// NOTE(keir): machine_epsilon() and precision() are missing, because they don't +// work with nested template types (e.g. where the scalar is itself templated). +// Among other things, this means that decompositions of Jet's does not work, +// for example +// +// Matrix<Jet<T, N> ... > A, x, b; +// ... +// A.solve(b, &x) +// +// does not work and will fail with a strange compiler error. +// +// TODO(keir): This is an Eigen 2.0 limitation that is lifted in 3.0. When we +// switch to 3.0, also add the rest of the specialization functionality. +template<typename T, int N> inline const Jet<T, N>& ei_conj(const Jet<T, N>& x) { return x; } // NOLINT +template<typename T, int N> inline const Jet<T, N>& ei_real(const Jet<T, N>& x) { return x; } // NOLINT +template<typename T, int N> inline Jet<T, N> ei_imag(const Jet<T, N>& ) { return Jet<T, N>(0.0); } // NOLINT +template<typename T, int N> inline Jet<T, N> ei_abs (const Jet<T, N>& x) { return fabs(x); } // NOLINT +template<typename T, int N> inline Jet<T, N> ei_abs2(const Jet<T, N>& x) { return x * x; } // NOLINT +template<typename T, int N> inline Jet<T, N> ei_sqrt(const Jet<T, N>& x) { return sqrt(x); } // NOLINT +template<typename T, int N> inline Jet<T, N> ei_exp (const Jet<T, N>& x) { return exp(x); } // NOLINT +template<typename T, int N> inline Jet<T, N> ei_log (const Jet<T, N>& x) { return log(x); } // NOLINT +template<typename T, int N> inline Jet<T, N> ei_sin (const Jet<T, N>& x) { return sin(x); } // NOLINT +template<typename T, int N> inline Jet<T, N> ei_cos (const Jet<T, N>& x) { return cos(x); } // NOLINT +template<typename T, int N> inline Jet<T, N> ei_pow (const Jet<T, N>& x, Jet<T, N> y) { return pow(x, y); } // NOLINT + +// Note: This has to be in the ceres namespace for argument dependent lookup to +// function correctly. Otherwise statements like CHECK_LE(x, 2.0) fail with +// strange compile errors. +template <typename T, int N> +inline std::ostream &operator<<(std::ostream &s, const Jet<T, N>& z) { + return s << "[" << z.a << " ; " << z.v.transpose() << "]"; +} + +// A jet traits class to make it easier to work with mixed auto / numeric diff. +template<typename T> +struct JetOps { + static bool IsScalar() { + return true; + } + static T GetScalar(const T& t) { + return t; + } + static void SetScalar(const T& scalar, T* t) { + *t = scalar; + } + static void ScaleDerivative(double scale_by, T *value) { + // For double, there is no derivative to scale. + } +}; + +template<typename T, int N> +struct JetOps<Jet<T, N> > { + static bool IsScalar() { + return false; + } + static T GetScalar(const Jet<T, N>& t) { + return t.a; + } + static void SetScalar(const T& scalar, Jet<T, N>* t) { + t->a = scalar; + } + static void ScaleDerivative(double scale_by, Jet<T, N> *value) { + value->v *= scale_by; + } +}; + +template<typename FunctionType, int kNumArgs, typename ArgumentType> +struct Chain { + static ArgumentType Rule(const FunctionType &f, + const FunctionType dfdx[kNumArgs], + const ArgumentType x[kNumArgs]) { + // In the default case of scalars, there's nothing to do since there are no + // derivatives to propagate. + return f; + } +}; + +// XXX Add documentation here! +template<typename FunctionType, int kNumArgs, typename T, int N> +struct Chain<FunctionType, kNumArgs, Jet<T, N> > { + static Jet<T, N> Rule(const FunctionType &f, + const FunctionType dfdx[kNumArgs], + const Jet<T, N> x[kNumArgs]) { + // x is itself a function of another variable ("z"); what this function + // needs to return is "f", but with the derivative with respect to z + // attached to the jet. So combine the derivative part of x's jets to form + // a Jacobian matrix between x and z (i.e. dx/dz). + Eigen::Matrix<T, kNumArgs, N> dxdz; + for (int i = 0; i < kNumArgs; ++i) { + dxdz.row(i) = x[i].v.transpose(); + } + + // Map the input gradient dfdx into an Eigen row vector. + Eigen::Map<const Eigen::Matrix<FunctionType, 1, kNumArgs> > + vector_dfdx(dfdx, 1, kNumArgs); + + // Now apply the chain rule to obtain df/dz. Combine the derivative with + // the scalar part to obtain f with full derivative information. + Jet<T, N> jet_f; + jet_f.a = f; + jet_f.v = vector_dfdx.template cast<T>() * dxdz; // Also known as dfdz. + return jet_f; + } +}; + +} // namespace ceres + +namespace Eigen { + +// Creating a specialization of NumTraits enables placing Jet objects inside +// Eigen arrays, getting all the goodness of Eigen combined with autodiff. +template<typename T, int N> +struct NumTraits<ceres::Jet<T, N> > { + typedef ceres::Jet<T, N> Real; + typedef ceres::Jet<T, N> NonInteger; + typedef ceres::Jet<T, N> Nested; + + static typename ceres::Jet<T, N> dummy_precision() { + return ceres::Jet<T, N>(1e-12); + } + + enum { + IsComplex = 0, + IsInteger = 0, + IsSigned, + ReadCost = 1, + AddCost = 1, + // For Jet types, multiplication is more expensive than addition. + MulCost = 3, + HasFloatingPoint = 1 + }; +}; + +} // namespace Eigen + +#endif // CERES_PUBLIC_JET_H_ diff --git a/extern/libmv/third_party/ceres/include/ceres/local_parameterization.h b/extern/libmv/third_party/ceres/include/ceres/local_parameterization.h new file mode 100644 index 00000000000..c0f7dc77a57 --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/local_parameterization.h @@ -0,0 +1,189 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_PUBLIC_LOCAL_PARAMETERIZATION_H_ +#define CERES_PUBLIC_LOCAL_PARAMETERIZATION_H_ + +#include <vector> +#include "ceres/internal/port.h" + +namespace ceres { + +// Purpose: Sometimes parameter blocks x can overparameterize a problem +// +// min f(x) +// x +// +// In that case it is desirable to choose a parameterization for the +// block itself to remove the null directions of the cost. More +// generally, if x lies on a manifold of a smaller dimension than the +// ambient space that it is embedded in, then it is numerically and +// computationally more effective to optimize it using a +// parameterization that lives in the tangent space of that manifold +// at each point. +// +// For example, a sphere in three dimensions is a 2 dimensional +// manifold, embedded in a three dimensional space. At each point on +// the sphere, the plane tangent to it defines a two dimensional +// tangent space. For a cost function defined on this sphere, given a +// point x, moving in the direction normal to the sphere at that point +// is not useful. Thus a better way to do a local optimization is to +// optimize over two dimensional vector delta in the tangent space at +// that point and then "move" to the point x + delta, where the move +// operation involves projecting back onto the sphere. Doing so +// removes a redundent dimension from the optimization, making it +// numerically more robust and efficient. +// +// More generally we can define a function +// +// x_plus_delta = Plus(x, delta), +// +// where x_plus_delta has the same size as x, and delta is of size +// less than or equal to x. The function Plus, generalizes the +// definition of vector addition. Thus it satisfies the identify +// +// Plus(x, 0) = x, for all x. +// +// A trivial version of Plus is when delta is of the same size as x +// and +// +// Plus(x, delta) = x + delta +// +// A more interesting case if x is two dimensional vector, and the +// user wishes to hold the first coordinate constant. Then, delta is a +// scalar and Plus is defined as +// +// Plus(x, delta) = x + [0] * delta +// [1] +// +// An example that occurs commonly in Structure from Motion problems +// is when camera rotations are parameterized using Quaternion. There, +// it is useful only make updates orthogonal to that 4-vector defining +// the quaternion. One way to do this is to let delta be a 3 +// dimensional vector and define Plus to be +// +// Plus(x, delta) = [cos(|delta|), sin(|delta|) delta / |delta|] * x +// +// The multiplication between the two 4-vectors on the RHS is the +// standard quaternion product. +// +// Given g and a point x, optimizing f can now be restated as +// +// min f(Plus(x, delta)) +// delta +// +// Given a solution delta to this problem, the optimal value is then +// given by +// +// x* = Plus(x, delta) +// +// The class LocalParameterization defines the function Plus and its +// Jacobian which is needed to compute the Jacobian of f w.r.t delta. +class LocalParameterization { + public: + virtual ~LocalParameterization() {} + + // Generalization of the addition operation, + // + // x_plus_delta = Plus(x, delta) + // + // with the condition that Plus(x, 0) = x. + virtual bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const = 0; + + // The jacobian of Plus(x, delta) w.r.t delta at delta = 0. + virtual bool ComputeJacobian(const double* x, double* jacobian) const = 0; + + // Size of x. + virtual int GlobalSize() const = 0; + + // Size of delta. + virtual int LocalSize() const = 0; +}; + +// Some basic parameterizations + +// Identity Parameterization: Plus(x, delta) = x + delta +class IdentityParameterization : public LocalParameterization { + public: + explicit IdentityParameterization(int size); + virtual ~IdentityParameterization() {} + virtual bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const; + virtual bool ComputeJacobian(const double* x, + double* jacobian) const; + virtual int GlobalSize() const { return size_; } + virtual int LocalSize() const { return size_; } + + private: + const int size_; +}; + +// Hold a subset of the parameters inside a parameter block constant. +class SubsetParameterization : public LocalParameterization { + public: + explicit SubsetParameterization(int size, + const vector<int>& constant_parameters); + virtual ~SubsetParameterization() {} + virtual bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const; + virtual bool ComputeJacobian(const double* x, + double* jacobian) const; + virtual int GlobalSize() const { return constancy_mask_.size(); } + virtual int LocalSize() const { return local_size_; } + + private: + const int local_size_; + vector<int> constancy_mask_; +}; + +// Plus(x, delta) = [cos(|delta|), sin(|delta|) delta / |delta|] * x +// with * being the quaternion multiplication operator. Here we assume +// that the first element of the quaternion vector is the real (cos +// theta) part. +class QuaternionParameterization : public LocalParameterization { + public: + virtual ~QuaternionParameterization() {} + virtual bool Plus(const double* x, + const double* delta, + double* x_plus_delta) const; + virtual bool ComputeJacobian(const double* x, + double* jacobian) const; + virtual int GlobalSize() const { return 4; } + virtual int LocalSize() const { return 3; } +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_LOCAL_PARAMETERIZATION_H_ diff --git a/extern/libmv/third_party/ceres/include/ceres/loss_function.h b/extern/libmv/third_party/ceres/include/ceres/loss_function.h new file mode 100644 index 00000000000..81add02cdee --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/loss_function.h @@ -0,0 +1,322 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// The LossFunction interface is the way users describe how residuals +// are converted to cost terms for the overall problem cost function. +// For the exact manner in which loss functions are converted to the +// overall cost for a problem, see problem.h. +// +// For least squares problem where there are no outliers and standard +// squared loss is expected, it is not necessary to create a loss +// function; instead passing a NULL to the problem when adding +// residuals implies a standard squared loss. +// +// For least squares problems where the minimization may encounter +// input terms that contain outliers, that is, completely bogus +// measurements, it is important to use a loss function that reduces +// their associated penalty. +// +// Consider a structure from motion problem. The unknowns are 3D +// points and camera parameters, and the measurements are image +// coordinates describing the expected reprojected position for a +// point in a camera. For example, we want to model the geometry of a +// street scene with fire hydrants and cars, observed by a moving +// camera with unknown parameters, and the only 3D points we care +// about are the pointy tippy-tops of the fire hydrants. Our magic +// image processing algorithm, which is responsible for producing the +// measurements that are input to Ceres, has found and matched all +// such tippy-tops in all image frames, except that in one of the +// frame it mistook a car's headlight for a hydrant. If we didn't do +// anything special (i.e. if we used a basic quadratic loss), the +// residual for the erroneous measurement will result in extreme error +// due to the quadratic nature of squared loss. This results in the +// entire solution getting pulled away from the optimimum to reduce +// the large error that would otherwise be attributed to the wrong +// measurement. +// +// Using a robust loss function, the cost for large residuals is +// reduced. In the example above, this leads to outlier terms getting +// downweighted so they do not overly influence the final solution. +// +// What cost function is best? +// +// In general, there isn't a principled way to select a robust loss +// function. The authors suggest starting with a non-robust cost, then +// only experimenting with robust loss functions if standard squared +// loss doesn't work. + +#ifndef CERES_PUBLIC_LOSS_FUNCTION_H_ +#define CERES_PUBLIC_LOSS_FUNCTION_H_ + +#include <glog/logging.h> +#include "ceres/internal/macros.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { + +class LossFunction { + public: + virtual ~LossFunction() {} + + // For a residual vector with squared 2-norm 'sq_norm', this method + // is required to fill in the value and derivatives of the loss + // function (rho in this example): + // + // out[0] = rho(sq_norm), + // out[1] = rho'(sq_norm), + // out[2] = rho''(sq_norm), + // + // Here the convention is that the contribution of a term to the + // cost function is given by 1/2 rho(s), where + // + // s = ||residuals||^2. + // + // Calling the method with a negative value of 's' is an error and + // the implementations are not required to handle that case. + // + // Most sane choices of rho() satisfy: + // + // rho(0) = 0, + // rho'(0) = 1, + // rho'(s) < 1 in outlier region, + // rho''(s) < 0 in outlier region, + // + // so that they mimic the least squares cost for small residuals. + virtual void Evaluate(double sq_norm, double out[3]) const = 0; +}; + +// Some common implementations follow below. +// +// Note: in the region of interest (i.e. s < 3) we have: +// TrivialLoss >= HuberLoss >= SoftLOneLoss >= CauchyLoss + + +// This corresponds to no robustification. +// +// rho(s) = s +// +// At s = 0: rho = [0, 1, 0]. +// +// It is not normally necessary to use this, as passing NULL for the +// loss function when building the problem accomplishes the same +// thing. +class TrivialLoss : public LossFunction { + public: + virtual void Evaluate(double, double*) const; +}; + +// Scaling +// ------- +// Given one robustifier +// s -> rho(s) +// one can change the length scale at which robustification takes +// place, by adding a scale factor 'a' as follows: +// +// s -> a^2 rho(s / a^2). +// +// The first and second derivatives are: +// +// s -> rho'(s / a^2), +// s -> (1 / a^2) rho''(s / a^2), +// +// but the behaviour near s = 0 is the same as the original function, +// i.e. +// +// rho(s) = s + higher order terms, +// a^2 rho(s / a^2) = s + higher order terms. +// +// The scalar 'a' should be positive. +// +// The reason for the appearance of squaring is that 'a' is in the +// units of the residual vector norm whereas 's' is a squared +// norm. For applications it is more convenient to specify 'a' than +// its square. The commonly used robustifiers below are described in +// un-scaled format (a = 1) but their implementations work for any +// non-zero value of 'a'. + +// Huber. +// +// rho(s) = s for s <= 1, +// rho(s) = 2 sqrt(s) - 1 for s >= 1. +// +// At s = 0: rho = [0, 1, 0]. +// +// The scaling parameter 'a' corresponds to 'delta' on this page: +// http://en.wikipedia.org/wiki/Huber_Loss_Function +class HuberLoss : public LossFunction { + public: + explicit HuberLoss(double a) : a_(a), b_(a * a) { } + virtual void Evaluate(double, double*) const; + private: + const double a_; + // b = a^2. + const double b_; +}; + +// Soft L1, similar to Huber but smooth. +// +// rho(s) = 2 (sqrt(1 + s) - 1). +// +// At s = 0: rho = [0, 1, -1/2]. +class SoftLOneLoss : public LossFunction { + public: + explicit SoftLOneLoss(double a) : b_(a * a), c_(1 / b_) { } + virtual void Evaluate(double, double*) const; + private: + // b = a^2. + const double b_; + // c = 1 / a^2. + const double c_; +}; + +// Inspired by the Cauchy distribution +// +// rho(s) = log(1 + s). +// +// At s = 0: rho = [0, 1, -1]. +class CauchyLoss : public LossFunction { + public: + explicit CauchyLoss(double a) : b_(a * a), c_(1 / b_) { } + virtual void Evaluate(double, double*) const; + private: + // b = a^2. + const double b_; + // c = 1 / a^2. + const double c_; +}; + +// The discussion above has to do with length scaling: it affects the space +// in which s is measured. Sometimes you want to simply scale the output +// value of the robustifier. For example, you might want to weight +// different error terms differently (e.g., weight pixel reprojection +// errors differently from terrain errors). +// +// If rho is the wrapped robustifier, then this simply outputs +// s -> a * rho(s) +// +// The first and second derivatives are, not surprisingly +// s -> a * rho'(s) +// s -> a * rho''(s) +// +// Since we treat the a NULL Loss function as the Identity loss +// function, rho = NULL is a valid input and will result in the input +// being scaled by a. This provides a simple way of implementing a +// scaled ResidualBlock. +class ScaledLoss : public LossFunction { + public: + // Constructs a ScaledLoss wrapping another loss function. Takes + // ownership of the wrapped loss function or not depending on the + // ownership parameter. + ScaledLoss(const LossFunction* rho, double a, Ownership ownership) : + rho_(rho), a_(a), ownership_(ownership) { } + + virtual ~ScaledLoss() { + if (ownership_ == DO_NOT_TAKE_OWNERSHIP) { + rho_.release(); + } + } + virtual void Evaluate(double, double*) const; + + private: + internal::scoped_ptr<const LossFunction> rho_; + const double a_; + const Ownership ownership_; + DISALLOW_COPY_AND_ASSIGN(ScaledLoss); +}; + +// Sometimes after the optimization problem has been constructed, we +// wish to mutate the scale of the loss function. For example, when +// performing estimation from data which has substantial outliers, +// convergence can be improved by starting out with a large scale, +// optimizing the problem and then reducing the scale. This can have +// better convergence behaviour than just using a loss function with a +// small scale. +// +// This templated class allows the user to implement a loss function +// whose scale can be mutated after an optimization problem has been +// constructed. +// +// Example usage +// +// Problem problem; +// +// // Add parameter blocks +// +// CostFunction* cost_function = +// new AutoDiffCostFunction < UW_Camera_Mapper, 2, 9, 3>( +// new UW_Camera_Mapper(data->observations[2*i + 0], +// data->observations[2*i + 1])); +// +// LossFunctionWrapper* loss_function(new HuberLoss(1.0), TAKE_OWNERSHIP); +// +// problem.AddResidualBlock(cost_function, loss_function, parameters); +// +// Solver::Options options; +// scoped_ptr<Solver::Summary> summary1(Solve(problem, options)); +// +// loss_function->Reset(new HuberLoss(1.0), TAKE_OWNERSHIP); +// +// scoped_ptr<Solver::Summary> summary2(Solve(problem, options)); +// +class LossFunctionWrapper : public LossFunction { + public: + LossFunctionWrapper(LossFunction* rho, Ownership ownership) + : rho_(rho), ownership_(ownership) { + } + + virtual ~LossFunctionWrapper() { + if (ownership_ == DO_NOT_TAKE_OWNERSHIP) { + rho_.release(); + } + } + + virtual void Evaluate(double sq_norm, double out[3]) const { + CHECK_NOTNULL(rho_.get()); + rho_->Evaluate(sq_norm, out); + } + + void Reset(LossFunction* rho, Ownership ownership) { + if (ownership_ == DO_NOT_TAKE_OWNERSHIP) { + rho_.release(); + } + rho_.reset(rho); + ownership_ = ownership; + } + + private: + internal::scoped_ptr<const LossFunction> rho_; + Ownership ownership_; + DISALLOW_COPY_AND_ASSIGN(LossFunctionWrapper); +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_LOSS_FUNCTION_H_ diff --git a/extern/libmv/third_party/ceres/include/ceres/normal_prior.h b/extern/libmv/third_party/ceres/include/ceres/normal_prior.h new file mode 100644 index 00000000000..480a07474a7 --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/normal_prior.h @@ -0,0 +1,75 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Cost term that implements a prior on a parameter block using a +// normal distribution. + +#ifndef CERES_PUBLIC_NORMAL_PRIOR_H_ +#define CERES_PUBLIC_NORMAL_PRIOR_H_ + +#include "ceres/cost_function.h" +#include "ceres/internal/eigen.h" + +namespace ceres { + +// Implements a cost function of the form +// +// cost(x) = ||A(x - b)||^2 +// +// where, the matrix A and the vector b are fixed and x is the +// variable. In case the user is interested in implementing a cost +// function of the form +// +// cost(x) = (x - mu)^T S^{-1} (x - mu) +// +// where, mu is a vector and S is a covariance matrix, then, A = +// S^{-1/2}, i.e the matrix A is the square root of the inverse of the +// covariance, also known as the stiffness matrix. There are however +// no restrictions on the shape of A. It is free to be rectangular, +// which would be the case if the covariance matrix S is rank +// deficient. + +class NormalPrior: public CostFunction { + public: + // Check that the number of rows in the vector b are the same as the + // number of columns in the matrix A, crash otherwise. + NormalPrior(const Matrix& A, const Vector& b); + + virtual bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const; + private: + Matrix A_; + Vector b_; +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_NORMAL_PRIOR_H_ diff --git a/extern/libmv/third_party/ceres/include/ceres/numeric_diff_cost_function.h b/extern/libmv/third_party/ceres/include/ceres/numeric_diff_cost_function.h new file mode 100644 index 00000000000..bbaefca5b6c --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/numeric_diff_cost_function.h @@ -0,0 +1,283 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// Create CostFunctions as needed by the least squares framework with jacobians +// computed via numeric (a.k.a. finite) differentiation. For more details see +// http://en.wikipedia.org/wiki/Numerical_differentiation. +// +// To get a numerically differentiated cost function, define a subclass of +// CostFunction such that the Evaluate() function ignores the jacobian +// parameter. The numeric differentiation wrapper will fill in the jacobian +// parameter if nececssary by repeatedly calling the Evaluate() function with +// small changes to the appropriate parameters, and computing the slope. For +// performance, the numeric differentiation wrapper class is templated on the +// concrete cost function, even though it could be implemented only in terms of +// the virtual CostFunction interface. +// +// The numerically differentiated version of a cost function for a cost function +// can be constructed as follows: +// +// CostFunction* cost_function +// = new NumericDiffCostFunction<MyCostFunction, CENTRAL, 1, 4, 8>( +// new MyCostFunction(...), TAKE_OWNERSHIP); +// +// where MyCostFunction has 1 residual and 2 parameter blocks with sizes 4 and 8 +// respectively. Look at the tests for a more detailed example. +// +// The central difference method is considerably more accurate at the cost of +// twice as many function evaluations than forward difference. Consider using +// central differences begin with, and only after that works, trying forward +// difference to improve performance. +// +// TODO(keir): Characterize accuracy; mention pitfalls; provide alternatives. + +#ifndef CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_ +#define CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_ + +#include <cstring> +#include <glog/logging.h> +#include "Eigen/Dense" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/sized_cost_function.h" +#include "ceres/types.h" + +namespace ceres { + +enum NumericDiffMethod { + CENTRAL, + FORWARD +}; + +// This is split from the main class because C++ doesn't allow partial template +// specializations for member functions. The alternative is to repeat the main +// class for differing numbers of parameters, which is also unfortunate. +template <typename CostFunctionNoJacobian, + int num_residuals, + int parameter_block_size, + int parameter_block, + NumericDiffMethod method> +struct Differencer { + // Mutates parameters but must restore them before return. + static bool EvaluateJacobianForParameterBlock( + const CostFunctionNoJacobian *function, + double const* residuals_at_eval_point, + double **parameters, + double **jacobians) { + using Eigen::Map; + using Eigen::Matrix; + using Eigen::RowMajor; + + typedef Matrix<double, num_residuals, 1> ResidualVector; + typedef Matrix<double, parameter_block_size, 1> ParameterVector; + typedef Matrix<double, num_residuals, parameter_block_size, RowMajor> + JacobianMatrix; + + Map<JacobianMatrix> parameter_jacobian(jacobians[parameter_block], + num_residuals, + parameter_block_size); + + // Mutate 1 element at a time and then restore. + Map<ParameterVector> x_plus_delta(parameters[parameter_block], + parameter_block_size); + ParameterVector x(x_plus_delta); + + // TODO(keir): Pick a smarter number! In theory a good choice is sqrt(eps) * + // x, which for doubles means about 1e-8 * x. However, I have found this + // number too optimistic. This number should be exposed for users to change. + const double kRelativeStepSize = 1e-6; + + ParameterVector step_size = x.array().abs() * kRelativeStepSize; + + // To handle cases where a parameter is exactly zero, instead use the mean + // step_size for the other dimensions. + double fallback_step_size = step_size.sum() / step_size.rows(); + if (fallback_step_size == 0.0) { + // If all the parameters are zero, there's no good answer. Take + // kRelativeStepSize as a guess and hope for the best. + fallback_step_size = kRelativeStepSize; + } + + // For each parameter in the parameter block, use finite differences to + // compute the derivative for that parameter. + for (int j = 0; j < parameter_block_size; ++j) { + if (step_size(j) == 0.0) { + // The parameter is exactly zero, so compromise and use the mean + // step_size from the other parameters. This can break in many cases, + // but it's hard to pick a good number without problem specific + // knowledge. + step_size(j) = fallback_step_size; + } + x_plus_delta(j) = x(j) + step_size(j); + + double residuals[num_residuals]; // NOLINT + if (!function->Evaluate(parameters, residuals, NULL)) { + // Something went wrong; bail. + return false; + } + + // Compute this column of the jacobian in 3 steps: + // 1. Store residuals for the forward part. + // 2. Subtract residuals for the backward (or 0) part. + // 3. Divide out the run. + parameter_jacobian.col(j) = + Map<const ResidualVector>(residuals, num_residuals); + + double one_over_h = 1 / step_size(j); + if (method == CENTRAL) { + // Compute the function on the other side of x(j). + x_plus_delta(j) = x(j) - step_size(j); + + if (!function->Evaluate(parameters, residuals, NULL)) { + // Something went wrong; bail. + return false; + } + parameter_jacobian.col(j) -= + Map<ResidualVector>(residuals, num_residuals, 1); + one_over_h /= 2; + } else { + // Forward difference only; reuse existing residuals evaluation. + parameter_jacobian.col(j) -= + Map<const ResidualVector>(residuals_at_eval_point, num_residuals); + } + x_plus_delta(j) = x(j); // Restore x_plus_delta. + + // Divide out the run to get slope. + parameter_jacobian.col(j) *= one_over_h; + } + return true; + } +}; + +// Prevent invalid instantiations. +template <typename CostFunctionNoJacobian, + int num_residuals, + int parameter_block, + NumericDiffMethod method> +struct Differencer<CostFunctionNoJacobian, + num_residuals, + 0 /* parameter_block_size */, + parameter_block, + method> { + static bool EvaluateJacobianForParameterBlock( + const CostFunctionNoJacobian *function, + double const* residuals_at_eval_point, + double **parameters, + double **jacobians) { + LOG(FATAL) << "Shouldn't get here."; + return true; + } +}; + +template <typename CostFunctionNoJacobian, + NumericDiffMethod method = CENTRAL, int M = 0, + int N0 = 0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0, int N5 = 0> +class NumericDiffCostFunction + : public SizedCostFunction<M, N0, N1, N2, N3, N4, N5> { + public: + NumericDiffCostFunction(CostFunctionNoJacobian* function, + Ownership ownership) + : function_(function), ownership_(ownership) {} + + virtual ~NumericDiffCostFunction() { + if (ownership_ != TAKE_OWNERSHIP) { + function_.release(); + } + } + + virtual bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const { + // Get the function value (residuals) at the the point to evaluate. + bool success = function_->Evaluate(parameters, residuals, NULL); + if (!success) { + // Something went wrong; ignore the jacobian. + return false; + } + if (!jacobians) { + // Nothing to do; just forward. + return true; + } + + // Create a copy of the parameters which will get mutated. + const int kParametersSize = N0 + N1 + N2 + N3 + N4 + N5; + double parameters_copy[kParametersSize]; + double *parameters_references_copy[6]; + parameters_references_copy[0] = ¶meters_copy[0]; + parameters_references_copy[1] = ¶meters_copy[0] + N0; + parameters_references_copy[2] = ¶meters_copy[0] + N0 + N1; + parameters_references_copy[3] = ¶meters_copy[0] + N0 + N1 + N2; + parameters_references_copy[4] = ¶meters_copy[0] + N0 + N1 + N2 + N3; + parameters_references_copy[5] = + ¶meters_copy[0] + N0 + N1 + N2 + N3 + N4; + +#define COPY_PARAMETER_BLOCK(block) \ + if (N ## block) memcpy(parameters_references_copy[block], \ + parameters[block], \ + sizeof(double) * N ## block); // NOLINT + COPY_PARAMETER_BLOCK(0); + COPY_PARAMETER_BLOCK(1); + COPY_PARAMETER_BLOCK(2); + COPY_PARAMETER_BLOCK(3); + COPY_PARAMETER_BLOCK(4); + COPY_PARAMETER_BLOCK(5); +#undef COPY_PARAMETER_BLOCK + +#define EVALUATE_JACOBIAN_FOR_BLOCK(block) \ + if (N ## block && jacobians[block]) { \ + if (!Differencer<CostFunctionNoJacobian, /* NOLINT */ \ + M, \ + N ## block, \ + block, \ + method>::EvaluateJacobianForParameterBlock( \ + function_.get(), \ + residuals, \ + parameters_references_copy, \ + jacobians)) { \ + return false; \ + } \ + } + EVALUATE_JACOBIAN_FOR_BLOCK(0); + EVALUATE_JACOBIAN_FOR_BLOCK(1); + EVALUATE_JACOBIAN_FOR_BLOCK(2); + EVALUATE_JACOBIAN_FOR_BLOCK(3); + EVALUATE_JACOBIAN_FOR_BLOCK(4); + EVALUATE_JACOBIAN_FOR_BLOCK(5); +#undef EVALUATE_JACOBIAN_FOR_BLOCK + return true; + } + + private: + internal::scoped_ptr<CostFunctionNoJacobian> function_; + Ownership ownership_; +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_NUMERIC_DIFF_COST_FUNCTION_H_ diff --git a/extern/libmv/third_party/ceres/include/ceres/problem.h b/extern/libmv/third_party/ceres/include/ceres/problem.h new file mode 100644 index 00000000000..0ca61006bdb --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/problem.h @@ -0,0 +1,265 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// keir@google.com (Keir Mierle) +// +// The Problem object is used to build and hold least squares problems. + +#ifndef CERES_PUBLIC_PROBLEM_H_ +#define CERES_PUBLIC_PROBLEM_H_ + +#include <cstddef> +#include <map> +#include <set> +#include <vector> + +#include <glog/logging.h> +#include "ceres/internal/macros.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { + +class CostFunction; +class LossFunction; +class LocalParameterization; + +namespace internal { +class Preprocessor; +class ProblemImpl; +class ParameterBlock; +class ResidualBlock; +class SolverImpl; +} // namespace internal + +// A ResidualBlockId is a handle clients can use to delete residual +// blocks after creating them. They are opaque for any purposes other +// than that. +typedef const internal::ResidualBlock* ResidualBlockId; + +// A class to represent non-linear least squares problems. Such +// problems have a cost function that is a sum of error terms (known +// as "residuals"), where each residual is a function of some subset +// of the parameters. The cost function takes the form +// +// N 1 +// SUM --- loss( || r_i1, r_i2,..., r_ik ||^2 ), +// i=1 2 +// +// where +// +// r_ij is residual number i, component j; the residual is a +// function of some subset of the parameters x1...xk. For +// example, in a structure from motion problem a residual +// might be the difference between a measured point in an +// image and the reprojected position for the matching +// camera, point pair. The residual would have two +// components, error in x and error in y. +// +// loss(y) is the loss function; for example, squared error or +// Huber L1 loss. If loss(y) = y, then the cost function is +// non-robustified least squares. +// +// This class is specifically designed to address the important subset +// of "sparse" least squares problems, where each component of the +// residual depends only on a small number number of parameters, even +// though the total number of residuals and parameters may be very +// large. This property affords tremendous gains in scale, allowing +// efficient solving of large problems that are otherwise +// inaccessible. +// +// The canonical example of a sparse least squares problem is +// "structure-from-motion" (SFM), where the parameters are points and +// cameras, and residuals are reprojection errors. Typically a single +// residual will depend only on 9 parameters (3 for the point, 6 for +// the camera). +// +// To create a least squares problem, use the AddResidualBlock() and +// AddParameterBlock() methods, documented below. Here is an example least +// squares problem containing 3 parameter blocks of sizes 3, 4 and 5 +// respectively and two residual terms of size 2 and 6: +// +// double x1[] = { 1.0, 2.0, 3.0 }; +// double x2[] = { 1.0, 2.0, 3.0, 5.0 }; +// double x3[] = { 1.0, 2.0, 3.0, 6.0, 7.0 }; +// +// Problem problem; +// +// problem.AddResidualBlock(new MyUnaryCostFunction(...), x1); +// problem.AddResidualBlock(new MyBinaryCostFunction(...), x2, x3); +// +// Please see cost_function.h for details of the CostFunction object. +class Problem { + public: + struct Options { + Options() + : cost_function_ownership(TAKE_OWNERSHIP), + loss_function_ownership(TAKE_OWNERSHIP), + local_parameterization_ownership(TAKE_OWNERSHIP) {} + + // These flags control whether the Problem object owns the cost + // functions, loss functions, and parameterizations passed into + // the Problem. If set to TAKE_OWNERSHIP, then the problem object + // will delete the corresponding cost or loss functions on + // destruction. The destructor is careful to delete the pointers + // only once, since sharing cost/loss/parameterizations is + // allowed. + Ownership cost_function_ownership; + Ownership loss_function_ownership; + Ownership local_parameterization_ownership; + }; + + // The default constructor is equivalent to the + // invocation Problem(Problem::Options()). + Problem(); + explicit Problem(const Options& options); + + ~Problem(); + + // Add a residual block to the overall cost function. The cost + // function carries with it information about the sizes of the + // parameter blocks it expects. The function checks that these match + // the sizes of the parameter blocks listed in parameter_blocks. The + // program aborts if a mismatch is detected. loss_function can be + // NULL, in which case the cost of the term is just the squared norm + // of the residuals. + // + // The user has the option of explicitly adding the parameter blocks + // using AddParameterBlock. This causes additional correctness + // checking; however, AddResidualBlock implicitly adds the parameter + // blocks if they are not present, so calling AddParameterBlock + // explicitly is not required. + // + // The Problem object by default takes ownership of the + // cost_function and loss_function pointers. These objects remain + // live for the life of the Problem object. If the user wishes to + // keep control over the destruction of these objects, then they can + // do this by setting the corresponding enums in the Options struct. + // + // Note: Even though the Problem takes ownership of cost_function + // and loss_function, it does not preclude the user from re-using + // them in another residual block. The destructor takes care to call + // delete on each cost_function or loss_function pointer only once, + // regardless of how many residual blocks refer to them. + // + // Example usage: + // + // double x1[] = {1.0, 2.0, 3.0}; + // double x2[] = {1.0, 2.0, 5.0, 6.0}; + // double x3[] = {3.0, 6.0, 2.0, 5.0, 1.0}; + // + // Problem problem; + // + // problem.AddResidualBlock(new MyUnaryCostFunction(...), NULL, x1); + // problem.AddResidualBlock(new MyBinaryCostFunction(...), NULL, x2, x1); + // + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + const vector<double*>& parameter_blocks); + + // Convenience methods for adding residuals with a small number of + // parameters. This is the common case. Instead of specifying the + // parameter block arguments as a vector, list them as pointers. + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, + double* x3); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, + double* x3, double* x4); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, + double* x3, double* x4, double* x5); + + // Add a parameter block with appropriate size to the problem. + // Repeated calls with the same arguments are ignored. Repeated + // calls with the same double pointer but a different size results + // in undefined behaviour. + void AddParameterBlock(double* values, int size); + + // Add a parameter block with appropriate size and parameterization + // to the problem. Repeated calls with the same arguments are + // ignored. Repeated calls with the same double pointer but a + // different size results in undefined behaviour. + void AddParameterBlock(double* values, + int size, + LocalParameterization* local_parameterization); + + // Hold the indicated parameter block constant during optimization. + void SetParameterBlockConstant(double* values); + + // Allow the indicated parameter to vary during optimization. + void SetParameterBlockVariable(double* values); + + // Set the local parameterization for one of the parameter blocks. + // The local_parameterization is owned by the Problem by default. It + // is acceptable to set the same parameterization for multiple + // parameters; the destructor is careful to delete local + // parameterizations only once. The local parameterization can only + // be set once per parameter, and cannot be changed once set. + void SetParameterization(double* values, + LocalParameterization* local_parameterization); + + // Number of parameter blocks in the problem. Always equals + // parameter_blocks().size() and parameter_block_sizes().size(). + int NumParameterBlocks() const; + + // The size of the parameter vector obtained by summing over the + // sizes of all the parameter blocks. + int NumParameters() const; + + // Number of residual blocks in the problem. Always equals + // residual_blocks().size(). + int NumResidualBlocks() const; + + // The size of the residual vector obtained by summing over the + // sizes of all of the residual blocks. + int NumResiduals() const; + + private: + friend class internal::SolverImpl; + internal::scoped_ptr<internal::ProblemImpl> problem_impl_; + DISALLOW_COPY_AND_ASSIGN(Problem); +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_PROBLEM_H_ diff --git a/extern/libmv/third_party/ceres/include/ceres/rotation.h b/extern/libmv/third_party/ceres/include/ceres/rotation.h new file mode 100644 index 00000000000..e4227e78b9a --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/rotation.h @@ -0,0 +1,526 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// sameeragarwal@google.com (Sameer Agarwal) +// +// Templated functions for manipulating rotations. The templated +// functions are useful when implementing functors for automatic +// differentiation. +// +// In the following, the Quaternions are laid out as 4-vectors, thus: +// +// q[0] scalar part. +// q[1] coefficient of i. +// q[2] coefficient of j. +// q[3] coefficient of k. +// +// where: i*i = j*j = k*k = -1 and i*j = k, j*k = i, k*i = j. + +#ifndef CERES_PUBLIC_ROTATION_H_ +#define CERES_PUBLIC_ROTATION_H_ + +#include <algorithm> +#include <cmath> + +namespace ceres { + +// Convert a value in combined axis-angle representation to a quaternion. +// The value angle_axis is a triple whose norm is an angle in radians, +// and whose direction is aligned with the axis of rotation, +// and quaternion is a 4-tuple that will contain the resulting quaternion. +// The implementation may be used with auto-differentiation up to the first +// derivative, higher derivatives may have unexpected results near the origin. +template<typename T> +void AngleAxisToQuaternion(T const* angle_axis, T* quaternion); + +// Convert a quaternion to the equivalent combined axis-angle representation. +// The value quaternion must be a unit quaternion - it is not normalized first, +// and angle_axis will be filled with a value whose norm is the angle of +// rotation in radians, and whose direction is the axis of rotation. +// The implemention may be used with auto-differentiation up to the first +// derivative, higher derivatives may have unexpected results near the origin. +template<typename T> +void QuaternionToAngleAxis(T const* quaternion, T* angle_axis); + +// Conversions between 3x3 rotation matrix (in column major order) and +// axis-angle rotation representations. Templated for use with +// autodifferentiation. +template <typename T> +void RotationMatrixToAngleAxis(T const * R, T * angle_axis); +template <typename T> +void AngleAxisToRotationMatrix(T const * angle_axis, T * R); + +// Conversions between 3x3 rotation matrix (in row major order) and +// Euler angle (in degrees) rotation representations. +// +// The {pitch,roll,yaw} Euler angles are rotations around the {x,y,z} +// axes, respectively. They are applied in that same order, so the +// total rotation R is Rz * Ry * Rx. +template <typename T> +void EulerAnglesToRotationMatrix(const T* euler, int row_stride, T* R); + +// Convert a 4-vector to a 3x3 scaled rotation matrix. +// +// The choice of rotation is such that the quaternion [1 0 0 0] goes to an +// identity matrix and for small a, b, c the quaternion [1 a b c] goes to +// the matrix +// +// [ 0 -c b ] +// I + 2 [ c 0 -a ] + higher order terms +// [ -b a 0 ] +// +// which corresponds to a Rodrigues approximation, the last matrix being +// the cross-product matrix of [a b c]. Together with the property that +// R(q1 * q2) = R(q1) * R(q2) this uniquely defines the mapping from q to R. +// +// The rotation matrix is row-major. +// +// No normalization of the quaternion is performed, i.e. +// R = ||q||^2 * Q, where Q is an orthonormal matrix +// such that det(Q) = 1 and Q*Q' = I +template <typename T> inline +void QuaternionToScaledRotation(const T q[4], T R[3 * 3]); + +// Same as above except that the rotation matrix is normalized by the +// Frobenius norm, so that R * R' = I (and det(R) = 1). +template <typename T> inline +void QuaternionToRotation(const T q[4], T R[3 * 3]); + +// Rotates a point pt by a quaternion q: +// +// result = R(q) * pt +// +// Assumes the quaternion is unit norm. This assumption allows us to +// write the transform as (something)*pt + pt, as is clear from the +// formula below. If you pass in a quaternion with |q|^2 = 2 then you +// WILL NOT get back 2 times the result you get for a unit quaternion. +template <typename T> inline +void UnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]); + +// With this function you do not need to assume that q has unit norm. +// It does assume that the norm is non-zero. +template <typename T> inline +void QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]); + +// zw = z * w, where * is the Quaternion product between 4 vectors. +template<typename T> inline +void QuaternionProduct(const T z[4], const T w[4], T zw[4]); + +// xy = x cross y; +template<typename T> inline +void CrossProduct(const T x[3], const T y[3], T x_cross_y[3]); + +template<typename T> inline +T DotProduct(const T x[3], const T y[3]); + +// y = R(angle_axis) * x; +template<typename T> inline +void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]); + +// --- IMPLEMENTATION + +// Duplicate rather than decorate every use of cmath with _USE_MATH_CONSTANTS. +// Necessitated by Windows. +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#define CERES_NEED_M_PI_UNDEF +#endif + +template<typename T> +inline void AngleAxisToQuaternion(const T* angle_axis, T* quaternion) { + const T &a0 = angle_axis[0]; + const T &a1 = angle_axis[1]; + const T &a2 = angle_axis[2]; + const T theta_squared = a0 * a0 + a1 * a1 + a2 * a2; + + // For points not at the origin, the full conversion is numerically stable. + if (theta_squared > T(0.0)) { + const T theta = sqrt(theta_squared); + const T half_theta = theta * T(0.5); + const T k = sin(half_theta) / theta; + quaternion[0] = cos(half_theta); + quaternion[1] = a0 * k; + quaternion[2] = a1 * k; + quaternion[3] = a2 * k; + } else { + // At the origin, sqrt() will produce NaN in the derivative since + // the argument is zero. By approximating with a Taylor series, + // and truncating at one term, the value and first derivatives will be + // computed correctly when Jets are used. + const T k(0.5); + quaternion[0] = T(1.0); + quaternion[1] = a0 * k; + quaternion[2] = a1 * k; + quaternion[3] = a2 * k; + } +} + +template<typename T> +inline void QuaternionToAngleAxis(const T* quaternion, T* angle_axis) { + const T &q1 = quaternion[1]; + const T &q2 = quaternion[2]; + const T &q3 = quaternion[3]; + const T sin_squared = q1 * q1 + q2 * q2 + q3 * q3; + + // For quaternions representing non-zero rotation, the conversion + // is numerically stable. + if (sin_squared > T(0.0)) { + const T sin_theta = sqrt(sin_squared); + const T k = T(2.0) * atan2(sin_theta, quaternion[0]) / sin_theta; + angle_axis[0] = q1 * k; + angle_axis[1] = q2 * k; + angle_axis[2] = q3 * k; + } else { + // For zero rotation, sqrt() will produce NaN in the derivative since + // the argument is zero. By approximating with a Taylor series, + // and truncating at one term, the value and first derivatives will be + // computed correctly when Jets are used. + const T k(2.0); + angle_axis[0] = q1 * k; + angle_axis[1] = q2 * k; + angle_axis[2] = q3 * k; + } +} + +// The conversion of a rotation matrix to the angle-axis form is +// numerically problematic when then rotation angle is close to zero +// or to Pi. The following implementation detects when these two cases +// occurs and deals with them by taking code paths that are guaranteed +// to not perform division by a small number. +template <typename T> +inline void RotationMatrixToAngleAxis(const T * R, T * angle_axis) { + // x = k * 2 * sin(theta), where k is the axis of rotation. + angle_axis[0] = R[5] - R[7]; + angle_axis[1] = R[6] - R[2]; + angle_axis[2] = R[1] - R[3]; + + static const T kOne = T(1.0); + static const T kTwo = T(2.0); + + // Since the right hand side may give numbers just above 1.0 or + // below -1.0 leading to atan misbehaving, we threshold. + T costheta = std::min(std::max((R[0] + R[4] + R[8] - kOne) / kTwo, + T(-1.0)), + kOne); + + // sqrt is guaranteed to give non-negative results, so we only + // threshold above. + T sintheta = std::min(sqrt(angle_axis[0] * angle_axis[0] + + angle_axis[1] * angle_axis[1] + + angle_axis[2] * angle_axis[2]) / kTwo, + kOne); + + // Use the arctan2 to get the right sign on theta + const T theta = atan2(sintheta, costheta); + + // Case 1: sin(theta) is large enough, so dividing by it is not a + // problem. We do not use abs here, because while jets.h imports + // std::abs into the namespace, here in this file, abs resolves to + // the int version of the function, which returns zero always. + // + // We use a threshold much larger then the machine epsilon, because + // if sin(theta) is small, not only do we risk overflow but even if + // that does not occur, just dividing by a small number will result + // in numerical garbage. So we play it safe. + static const double kThreshold = 1e-12; + if ((sintheta > kThreshold) || (sintheta < -kThreshold)) { + const T r = theta / (kTwo * sintheta); + for (int i = 0; i < 3; ++i) { + angle_axis[i] *= r; + } + return; + } + + // Case 2: theta ~ 0, means sin(theta) ~ theta to a good + // approximation. + if (costheta > 0) { + const T kHalf = T(0.5); + for (int i = 0; i < 3; ++i) { + angle_axis[i] *= kHalf; + } + return; + } + + // Case 3: theta ~ pi, this is the hard case. Since theta is large, + // and sin(theta) is small. Dividing by theta by sin(theta) will + // either give an overflow or worse still numerically meaningless + // results. Thus we use an alternate more complicated formula + // here. + + // Since cos(theta) is negative, division by (1-cos(theta)) cannot + // overflow. + const T inv_one_minus_costheta = kOne / (kOne - costheta); + + // We now compute the absolute value of coordinates of the axis + // vector using the diagonal entries of R. To resolve the sign of + // these entries, we compare the sign of angle_axis[i]*sin(theta) + // with the sign of sin(theta). If they are the same, then + // angle_axis[i] should be positive, otherwise negative. + for (int i = 0; i < 3; ++i) { + angle_axis[i] = theta * sqrt((R[i*4] - costheta) * inv_one_minus_costheta); + if (((sintheta < 0) && (angle_axis[i] > 0)) || + ((sintheta > 0) && (angle_axis[i] < 0))) { + angle_axis[i] = -angle_axis[i]; + } + } +} + +template <typename T> +inline void AngleAxisToRotationMatrix(const T * angle_axis, T * R) { + static const T kOne = T(1.0); + const T theta2 = DotProduct(angle_axis, angle_axis); + if (theta2 > 0.0) { + // We want to be careful to only evaluate the square root if the + // norm of the angle_axis vector is greater than zero. Otherwise + // we get a division by zero. + const T theta = sqrt(theta2); + const T wx = angle_axis[0] / theta; + const T wy = angle_axis[1] / theta; + const T wz = angle_axis[2] / theta; + + const T costheta = cos(theta); + const T sintheta = sin(theta); + + R[0] = costheta + wx*wx*(kOne - costheta); + R[1] = wz*sintheta + wx*wy*(kOne - costheta); + R[2] = -wy*sintheta + wx*wz*(kOne - costheta); + R[3] = wx*wy*(kOne - costheta) - wz*sintheta; + R[4] = costheta + wy*wy*(kOne - costheta); + R[5] = wx*sintheta + wy*wz*(kOne - costheta); + R[6] = wy*sintheta + wx*wz*(kOne - costheta); + R[7] = -wx*sintheta + wy*wz*(kOne - costheta); + R[8] = costheta + wz*wz*(kOne - costheta); + } else { + // At zero, we switch to using the first order Taylor expansion. + R[0] = kOne; + R[1] = -angle_axis[2]; + R[2] = angle_axis[1]; + R[3] = angle_axis[2]; + R[4] = kOne; + R[5] = -angle_axis[0]; + R[6] = -angle_axis[1]; + R[7] = angle_axis[0]; + R[8] = kOne; + } +} + +template <typename T> +inline void EulerAnglesToRotationMatrix(const T* euler, + const int row_stride, + T* R) { + const T degrees_to_radians(M_PI / 180.0); + + const T pitch(euler[0] * degrees_to_radians); + const T roll(euler[1] * degrees_to_radians); + const T yaw(euler[2] * degrees_to_radians); + + const T c1 = cos(yaw); + const T s1 = sin(yaw); + const T c2 = cos(roll); + const T s2 = sin(roll); + const T c3 = cos(pitch); + const T s3 = sin(pitch); + + // Rows of the rotation matrix. + T* R1 = R; + T* R2 = R1 + row_stride; + T* R3 = R2 + row_stride; + + R1[0] = c1*c2; + R1[1] = -s1*c3 + c1*s2*s3; + R1[2] = s1*s3 + c1*s2*c3; + + R2[0] = s1*c2; + R2[1] = c1*c3 + s1*s2*s3; + R2[2] = -c1*s3 + s1*s2*c3; + + R3[0] = -s2; + R3[1] = c2*s3; + R3[2] = c2*c3; +} + +template <typename T> inline +void QuaternionToScaledRotation(const T q[4], T R[3 * 3]) { + // Make convenient names for elements of q. + T a = q[0]; + T b = q[1]; + T c = q[2]; + T d = q[3]; + // This is not to eliminate common sub-expression, but to + // make the lines shorter so that they fit in 80 columns! + T aa = a * a; + T ab = a * b; + T ac = a * c; + T ad = a * d; + T bb = b * b; + T bc = b * c; + T bd = b * d; + T cc = c * c; + T cd = c * d; + T dd = d * d; + + R[0] = aa + bb - cc - dd; R[1] = T(2) * (bc - ad); R[2] = T(2) * (ac + bd); // NOLINT + R[3] = T(2) * (ad + bc); R[4] = aa - bb + cc - dd; R[5] = T(2) * (cd - ab); // NOLINT + R[6] = T(2) * (bd - ac); R[7] = T(2) * (ab + cd); R[8] = aa - bb - cc + dd; // NOLINT +} + +template <typename T> inline +void QuaternionToRotation(const T q[4], T R[3 * 3]) { + QuaternionToScaledRotation(q, R); + + T normalizer = q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]; + CHECK_NE(normalizer, T(0)); + normalizer = T(1) / normalizer; + + for (int i = 0; i < 9; ++i) { + R[i] *= normalizer; + } +} + +template <typename T> inline +void UnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) { + const T t2 = q[0] * q[1]; + const T t3 = q[0] * q[2]; + const T t4 = q[0] * q[3]; + const T t5 = -q[1] * q[1]; + const T t6 = q[1] * q[2]; + const T t7 = q[1] * q[3]; + const T t8 = -q[2] * q[2]; + const T t9 = q[2] * q[3]; + const T t1 = -q[3] * q[3]; + result[0] = T(2) * ((t8 + t1) * pt[0] + (t6 - t4) * pt[1] + (t3 + t7) * pt[2]) + pt[0]; // NOLINT + result[1] = T(2) * ((t4 + t6) * pt[0] + (t5 + t1) * pt[1] + (t9 - t2) * pt[2]) + pt[1]; // NOLINT + result[2] = T(2) * ((t7 - t3) * pt[0] + (t2 + t9) * pt[1] + (t5 + t8) * pt[2]) + pt[2]; // NOLINT +} + + +template <typename T> inline +void QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) { + // 'scale' is 1 / norm(q). + const T scale = T(1) / sqrt(q[0] * q[0] + + q[1] * q[1] + + q[2] * q[2] + + q[3] * q[3]); + + // Make unit-norm version of q. + const T unit[4] = { + scale * q[0], + scale * q[1], + scale * q[2], + scale * q[3], + }; + + UnitQuaternionRotatePoint(unit, pt, result); +} + +template<typename T> inline +void QuaternionProduct(const T z[4], const T w[4], T zw[4]) { + zw[0] = z[0] * w[0] - z[1] * w[1] - z[2] * w[2] - z[3] * w[3]; + zw[1] = z[0] * w[1] + z[1] * w[0] + z[2] * w[3] - z[3] * w[2]; + zw[2] = z[0] * w[2] - z[1] * w[3] + z[2] * w[0] + z[3] * w[1]; + zw[3] = z[0] * w[3] + z[1] * w[2] - z[2] * w[1] + z[3] * w[0]; +} + +// xy = x cross y; +template<typename T> inline +void CrossProduct(const T x[3], const T y[3], T x_cross_y[3]) { + x_cross_y[0] = x[1] * y[2] - x[2] * y[1]; + x_cross_y[1] = x[2] * y[0] - x[0] * y[2]; + x_cross_y[2] = x[0] * y[1] - x[1] * y[0]; +} + +template<typename T> inline +T DotProduct(const T x[3], const T y[3]) { + return (x[0] * y[0] + x[1] * y[1] + x[2] * y[2]); +} + +template<typename T> inline +void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]) { + T w[3]; + T sintheta; + T costheta; + + const T theta2 = DotProduct(angle_axis, angle_axis); + if (theta2 > 0.0) { + // Away from zero, use the rodriguez formula + // + // result = pt costheta + + // (w x pt) * sintheta + + // w (w . pt) (1 - costheta) + // + // We want to be careful to only evaluate the square root if the + // norm of the angle_axis vector is greater than zero. Otherwise + // we get a division by zero. + // + const T theta = sqrt(theta2); + w[0] = angle_axis[0] / theta; + w[1] = angle_axis[1] / theta; + w[2] = angle_axis[2] / theta; + costheta = cos(theta); + sintheta = sin(theta); + T w_cross_pt[3]; + CrossProduct(w, pt, w_cross_pt); + T w_dot_pt = DotProduct(w, pt); + for (int i = 0; i < 3; ++i) { + result[i] = pt[i] * costheta + + w_cross_pt[i] * sintheta + + w[i] * (T(1.0) - costheta) * w_dot_pt; + } + } else { + // Near zero, the first order Taylor approximation of the rotation + // matrix R corresponding to a vector w and angle w is + // + // R = I + hat(w) * sin(theta) + // + // But sintheta ~ theta and theta * w = angle_axis, which gives us + // + // R = I + hat(w) + // + // and actually performing multiplication with the point pt, gives us + // R * pt = pt + w x pt. + // + // Switching to the Taylor expansion at zero helps avoid all sorts + // of numerical nastiness. + T w_cross_pt[3]; + CrossProduct(angle_axis, pt, w_cross_pt); + for (int i = 0; i < 3; ++i) { + result[i] = pt[i] + w_cross_pt[i]; + } + } +} + +} // namespace ceres + +// Clean define pollution. +#ifdef CERES_NEED_M_PI_UNDEF +#undef CERES_NEED_M_PI_UNDEF +#undef M_PI +#endif + +#endif // CERES_PUBLIC_ROTATION_H_ diff --git a/extern/libmv/third_party/ceres/include/ceres/sized_cost_function.h b/extern/libmv/third_party/ceres/include/ceres/sized_cost_function.h new file mode 100644 index 00000000000..2894a9fba5c --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/sized_cost_function.h @@ -0,0 +1,88 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// A convenience class for cost functions which are statically sized. +// Compared to the dynamically-sized base class, this reduces boilerplate. +// +// The kNumResiduals template parameter can be a constant such as 2 or 5, or it +// can be ceres::DYNAMIC. If kNumResiduals is ceres::DYNAMIC, then subclasses +// are responsible for calling set_num_residuals() at runtime. + +#ifndef CERES_PUBLIC_SIZED_COST_FUNCTION_H_ +#define CERES_PUBLIC_SIZED_COST_FUNCTION_H_ + +#include <glog/logging.h> +#include "ceres/types.h" +#include "ceres/cost_function.h" + +namespace ceres { + +template<int kNumResiduals, + int N0 = 0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0, int N5 = 0> +class SizedCostFunction : public CostFunction { + public: + SizedCostFunction() { + // Sanity checking. + CHECK(kNumResiduals > 0 || kNumResiduals == DYNAMIC) + << "Cost functions must have at least one residual block."; + + CHECK_GT(N0, 0) + << "Cost functions must have at least one parameter block."; + CHECK((!N1 && !N2 && !N3 && !N4 && !N5) || + ((N1 > 0) && !N2 && !N3 && !N4 && !N5) || + ((N1 > 0) && (N2 > 0) && !N3 && !N4 && !N5) || + ((N1 > 0) && (N2 > 0) && (N3 > 0) && !N4 && !N5) || + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && !N5) || + ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0))) + << "Zero block cannot precede a non-zero block. Block sizes are " + << "(ignore trailing 0s): " << N0 << ", " << N1 << ", " << N2 << ", " + << N3 << ", " << N4 << ", " << N5; + + set_num_residuals(kNumResiduals); + +#define ADD_PARAMETER_BLOCK(N) \ + if (N) mutable_parameter_block_sizes()->push_back(N); + ADD_PARAMETER_BLOCK(N0); + ADD_PARAMETER_BLOCK(N1); + ADD_PARAMETER_BLOCK(N2); + ADD_PARAMETER_BLOCK(N3); + ADD_PARAMETER_BLOCK(N4); + ADD_PARAMETER_BLOCK(N5); +#undef ADD_PARAMETER_BLOCK + } + + virtual ~SizedCostFunction() { } + + // Subclasses must implement Evaluate(). +}; + +} // namespace ceres + +#endif // CERES_PUBLIC_SIZED_COST_FUNCTION_H_ diff --git a/extern/libmv/third_party/ceres/include/ceres/solver.h b/extern/libmv/third_party/ceres/include/ceres/solver.h new file mode 100644 index 00000000000..bd669272023 --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/solver.h @@ -0,0 +1,376 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_PUBLIC_SOLVER_H_ +#define CERES_PUBLIC_SOLVER_H_ + +#include <cmath> +#include <string> +#include <vector> + +#include "ceres/iteration_callback.h" +#include "ceres/internal/macros.h" +#include "ceres/internal/port.h" +#include "ceres/types.h" + +namespace ceres { + +class Problem; + +// Interface for non-linear least squares solvers. +class Solver { + public: + virtual ~Solver(); + + // The options structure contains, not surprisingly, options that control how + // the solver operates. The defaults should be suitable for a wide range of + // problems; however, better performance is often obtainable with tweaking. + // + // The constants are defined inside types.h + struct Options { + // Default constructor that sets up a generic sparse problem. + Options() { + minimizer_type = LEVENBERG_MARQUARDT; + max_num_iterations = 50; + max_solver_time_sec = 1.0e9; + num_threads = 1; + tau = 1e-4; + min_relative_decrease = 1e-3; + function_tolerance = 1e-6; + gradient_tolerance = 1e-10; + parameter_tolerance = 1e-8; +#ifndef CERES_NO_SUITESPARSE + linear_solver_type = SPARSE_NORMAL_CHOLESKY; +#else + linear_solver_type = DENSE_QR; +#endif // CERES_NO_SUITESPARSE + preconditioner_type = JACOBI; + num_linear_solver_threads = 1; + num_eliminate_blocks = 0; + ordering_type = NATURAL; + linear_solver_min_num_iterations = 1; + linear_solver_max_num_iterations = 500; + eta = 1e-1; + jacobi_scaling = true; + logging_type = PER_MINIMIZER_ITERATION; + minimizer_progress_to_stdout = false; + return_initial_residuals = false; + return_final_residuals = false; + lsqp_dump_directory = "/tmp"; + lsqp_dump_format_type = TEXTFILE; + crash_and_dump_lsqp_on_failure = false; + check_gradients = false; + gradient_check_relative_precision = 1e-8; + numeric_derivative_relative_step_size = 1e-6; + update_state_every_iteration = false; + } + + // Minimizer options ---------------------------------------- + + MinimizerType minimizer_type; + + // Maximum number of iterations for the minimizer to run for. + int max_num_iterations; + + // Maximum time for which the minimizer should run for. + double max_solver_time_sec; + + // Number of threads used by Ceres for evaluating the cost and + // jacobians. + int num_threads; + + // For Levenberg-Marquardt, the initial value for the + // regularizer. This is the inversely related to the size of the + // initial trust region. + double tau; + + // For trust region methods, this is lower threshold for the + // relative decrease before a step is accepted. + double min_relative_decrease; + + // Minimizer terminates when + // + // (new_cost - old_cost) < function_tolerance * old_cost; + // + double function_tolerance; + + // Minimizer terminates when + // + // max_i |gradient_i| < gradient_tolerance * max_i|initial_gradient_i| + // + // This value should typically be 1e-4 * function_tolerance. + double gradient_tolerance; + + // Minimizer terminates when + // + // |step|_2 <= parameter_tolerance * ( |x|_2 + parameter_tolerance) + // + double parameter_tolerance; + + // Linear least squares solver options ------------------------------------- + + LinearSolverType linear_solver_type; + + // Type of preconditioner to use with the iterative linear solvers. + PreconditionerType preconditioner_type; + + // Number of threads used by Ceres to solve the Newton + // step. Currently only the SPARSE_SCHUR solver is capable of + // using this setting. + int num_linear_solver_threads; + + // For Schur reduction based methods, the first 0 to num blocks are + // eliminated using the Schur reduction. For example, when solving + // traditional structure from motion problems where the parameters are in + // two classes (cameras and points) then num_eliminate_blocks would be the + // number of points. + // + // This parameter is used in conjunction with the ordering. + // Applies to: Preprocessor and linear least squares solver. + int num_eliminate_blocks; + + // Internally Ceres reorders the parameter blocks to help the + // various linear solvers. This parameter allows the user to + // influence the re-ordering strategy used. For structure from + // motion problems use SCHUR, for other problems NATURAL (default) + // is a good choice. In case you wish to specify your own ordering + // scheme, for example in conjunction with num_eliminate_blocks, + // use USER. + OrderingType ordering_type; + + // The ordering of the parameter blocks. The solver pays attention + // to it if the ordering_type is set to USER and the vector is + // non-empty. + vector<double*> ordering; + + + // Minimum number of iterations for which the linear solver should + // run, even if the convergence criterion is satisfied. + int linear_solver_min_num_iterations; + + // Maximum number of iterations for which the linear solver should + // run. If the solver does not converge in less than + // linear_solver_max_num_iterations, then it returns + // MAX_ITERATIONS, as its termination type. + int linear_solver_max_num_iterations; + + // Forcing sequence parameter. The truncated Newton solver uses + // this number to control the relative accuracy with which the + // Newton step is computed. + // + // This constant is passed to ConjugateGradientsSolver which uses + // it to terminate the iterations when + // + // (Q_i - Q_{i-1})/Q_i < eta/i + double eta; + + // Normalize the jacobian using Jacobi scaling before calling + // the linear least squares solver. + bool jacobi_scaling; + + // Logging options --------------------------------------------------------- + + LoggingType logging_type; + + // By default the Minimizer progress is logged to VLOG(1), which + // is sent to STDERR depending on the vlog level. If this flag is + // set to true, and logging_type is not SILENT, the logging output + // is sent to STDOUT. + bool minimizer_progress_to_stdout; + + bool return_initial_residuals; + bool return_final_residuals; + + // List of iterations at which the optimizer should dump the + // linear least squares problem to disk. Useful for testing and + // benchmarking. If empty (default), no problems are dumped. + // + // This is ignored if protocol buffers are disabled. + vector<int> lsqp_iterations_to_dump; + string lsqp_dump_directory; + DumpFormatType lsqp_dump_format_type; + + // Dump the linear least squares problem to disk if the minimizer + // fails due to NUMERICAL_FAILURE and crash the process. This flag + // is useful for generating debugging information. The problem is + // dumped in a file whose name is determined by + // Solver::Options::lsqp_dump_format. + // + // Note: This requires a version of Ceres built with protocol buffers. + bool crash_and_dump_lsqp_on_failure; + + // Finite differences options ---------------------------------------------- + + // Check all jacobians computed by each residual block with finite + // differences. This is expensive since it involves computing the + // derivative by normal means (e.g. user specified, autodiff, + // etc), then also computing it using finite differences. The + // results are compared, and if they differ substantially, details + // are printed to the log. + bool check_gradients; + + // Relative precision to check for in the gradient checker. If the + // relative difference between an element in a jacobian exceeds + // this number, then the jacobian for that cost term is dumped. + double gradient_check_relative_precision; + + // Relative shift used for taking numeric derivatives. For finite + // differencing, each dimension is evaluated at slightly shifted + // values; for the case of central difference, this is what gets + // evaluated: + // + // delta = numeric_derivative_relative_step_size; + // f_initial = f(x) + // f_forward = f((1 + delta) * x) + // f_backward = f((1 - delta) * x) + // + // The finite differencing is done along each dimension. The + // reason to use a relative (rather than absolute) step size is + // that this way, numeric differentation works for functions where + // the arguments are typically large (e.g. 1e9) and when the + // values are small (e.g. 1e-5). It is possible to construct + // "torture cases" which break this finite difference heuristic, + // but they do not come up often in practice. + // + // TODO(keir): Pick a smarter number than the default above! In + // theory a good choice is sqrt(eps) * x, which for doubles means + // about 1e-8 * x. However, I have found this number too + // optimistic. This number should be exposed for users to change. + double numeric_derivative_relative_step_size; + + // If true, the user's parameter blocks are updated at the end of + // every Minimizer iteration, otherwise they are updated when the + // Minimizer terminates. This is useful if, for example, the user + // wishes to visualize the state of the optimization every + // iteration. + bool update_state_every_iteration; + + // Callbacks that are executed at the end of each iteration of the + // Minimizer. They are executed in the order that they are + // specified in this vector. By default, parameter blocks are + // updated only at the end of the optimization, i.e when the + // Minimizer terminates. This behaviour is controlled by + // update_state_every_variable. If the user wishes to have access + // to the update parameter blocks when his/her callbacks are + // executed, then set update_state_every_iteration to true. + // + // The solver does NOT take ownership of these pointers. + vector<IterationCallback*> callbacks; + }; + + struct Summary { + Summary(); + + // A brief one line description of the state of the solver after + // termination. + string BriefReport() const; + + // A full multiline description of the state of the solver after + // termination. + string FullReport() const; + + // Minimizer summary ------------------------------------------------- + SolverTerminationType termination_type; + + // If the solver did not run, or there was a failure, a + // description of the error. + string error; + + // Cost of the problem before and after the optimization. See + // problem.h for definition of the cost of a problem. + double initial_cost; + double final_cost; + + // The part of the total cost that comes from residual blocks that + // were held fixed by the preprocessor because all the parameter + // blocks that they depend on were fixed. + double fixed_cost; + + // Residuals before and after the optimization. Each vector + // contains problem.NumResiduals() elements. Residuals are in the + // same order in which they were added to the problem object when + // constructing this problem. + vector<double> initial_residuals; + vector<double> final_residuals; + + vector<IterationSummary> iterations; + + int num_successful_steps; + int num_unsuccessful_steps; + + double preprocessor_time_in_seconds; + double minimizer_time_in_seconds; + double total_time_in_seconds; + + // Preprocessor summary. + int num_parameter_blocks; + int num_parameters; + int num_residual_blocks; + int num_residuals; + + int num_parameter_blocks_reduced; + int num_parameters_reduced; + int num_residual_blocks_reduced; + int num_residuals_reduced; + + int num_eliminate_blocks_given; + int num_eliminate_blocks_used; + + int num_threads_given; + int num_threads_used; + + int num_linear_solver_threads_given; + int num_linear_solver_threads_used; + + LinearSolverType linear_solver_type_given; + LinearSolverType linear_solver_type_used; + + PreconditionerType preconditioner_type; + OrderingType ordering_type; + }; + + // Once a least squares problem has been built, this function takes + // the problem and optimizes it based on the values of the options + // parameters. Upon return, a detailed summary of the work performed + // by the preprocessor, the non-linear minmizer and the linear + // solver are reported in the summary object. + virtual void Solve(const Options& options, + Problem* problem, + Solver::Summary* summary); +}; + +// Helper function which avoids going through the interface. +void Solve(const Solver::Options& options, + Problem* problem, + Solver::Summary* summary); + +} // namespace ceres + +#endif // CERES_PUBLIC_SOLVER_H_ diff --git a/extern/libmv/third_party/ceres/include/ceres/types.h b/extern/libmv/third_party/ceres/include/ceres/types.h new file mode 100644 index 00000000000..a30c79029ac --- /dev/null +++ b/extern/libmv/third_party/ceres/include/ceres/types.h @@ -0,0 +1,258 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Enums and other top level class definitions. +// +// Note: internal/types.cc defines stringification routines for some +// of these enums. Please update those routines if you extend or +// remove enums from here. + +#ifndef CERES_PUBLIC_TYPES_H_ +#define CERES_PUBLIC_TYPES_H_ + +namespace ceres { + +// Basic integer types. These typedefs are in the Ceres namespace to avoid +// conflicts with other packages having similar typedefs. +typedef short int16; +typedef int int32; + +// Argument type used in interfaces that can optionally take ownership +// of a passed in argument. If TAKE_OWNERSHIP is passed, the called +// object takes ownership of the pointer argument, and will call +// delete on it upon completion. +enum Ownership { + DO_NOT_TAKE_OWNERSHIP, + TAKE_OWNERSHIP +}; + +// TODO(keir): Considerably expand the explanations of each solver type. +enum LinearSolverType { + // These solvers are for general rectangular systems formed from the + // normal equations A'A x = A'b. They are direct solvers and do not + // assume any special problem structure. + + // Solve the normal equations using a sparse cholesky solver; based + // on CHOLMOD. + SPARSE_NORMAL_CHOLESKY, + + // Solve the normal equations using a dense QR solver; based on + // Eigen. + DENSE_QR, + + // Specialized solvers, specific to problems with a generalized + // bi-partitite structure. + + // Solves the reduced linear system using a dense Cholesky solver; + // based on Eigen. + DENSE_SCHUR, + + // Solves the reduced linear system using a sparse Cholesky solver; + // based on CHOLMOD. + SPARSE_SCHUR, + + // Solves the reduced linear system using Conjugate Gradients, based + // on a new Ceres implementation. Suitable for large scale + // problems. + ITERATIVE_SCHUR, + + // Conjugate gradients on the normal equations. + CGNR +}; + +enum PreconditionerType { + // Trivial preconditioner - the identity matrix. + IDENTITY, + + // Block diagonal of the Gauss-Newton Hessian. + JACOBI, + + // Block diagonal of the Schur complement. This preconditioner may + // only be used with the ITERATIVE_SCHUR solver. Requires + // SuiteSparse/CHOLMOD. + SCHUR_JACOBI, + + // Visibility clustering based preconditioners. + // + // These preconditioners are well suited for Structure from Motion + // problems, particularly problems arising from community photo + // collections. These preconditioners use the visibility structure + // of the scene to determine the sparsity structure of the + // preconditioner. Requires SuiteSparse/CHOLMOD. + CLUSTER_JACOBI, + CLUSTER_TRIDIAGONAL +}; + +enum LinearSolverTerminationType { + // Termination criterion was met. For factorization based solvers + // the tolerance is assumed to be zero. Any user provided values are + // ignored. + TOLERANCE, + + // Solver ran for max_num_iterations and terminated before the + // termination tolerance could be satified. + MAX_ITERATIONS, + + // Solver is stuck and further iterations will not result in any + // measurable progress. + STAGNATION, + + // Solver failed. Solver was terminated due to numerical errors. The + // exact cause of failure depends on the particular solver being + // used. + FAILURE +}; + +enum OrderingType { + // The order in which the parameter blocks were defined. + NATURAL, + + // Use the ordering specificed in the vector ordering. + USER, + + // Automatically figure out the best ordering to use the schur + // complement based solver. + SCHUR +}; + +// Logging options +// The options get progressively noisier. +enum LoggingType { + SILENT, + PER_MINIMIZER_ITERATION +}; + +enum MinimizerType { + LEVENBERG_MARQUARDT +}; + +enum SolverTerminationType { + // The minimizer did not run at all; usually due to errors in the user's + // Problem or the solver options. + DID_NOT_RUN, + + // The solver ran for maximum number of iterations specified by the + // user, but none of the convergence criterion specified by the user + // were met. + NO_CONVERGENCE, + + // Minimizer terminated because + // (new_cost - old_cost) < function_tolerance * old_cost; + FUNCTION_TOLERANCE, + + // Minimizer terminated because + // max_i |gradient_i| < gradient_tolerance * max_i|initial_gradient_i| + GRADIENT_TOLERANCE, + + // Minimized terminated because + // |step|_2 <= parameter_tolerance * ( |x|_2 + parameter_tolerance) + PARAMETER_TOLERANCE, + + // The minimizer terminated because it encountered a numerical error + // that it could not recover from. + NUMERICAL_FAILURE, + + // Using an IterationCallback object, user code can control the + // minimizer. The following enums indicate that the user code was + // responsible for termination. + + // User's IterationCallback returned SOLVER_ABORT. + USER_ABORT, + + // User's IterationCallback returned SOLVER_TERMINATE_SUCCESSFULLY + USER_SUCCESS +}; + +// Enums used by the IterationCallback instances to indicate to the +// solver whether it should continue solving, the user detected an +// error or the solution is good enough and the solver should +// terminate. +enum CallbackReturnType { + // Continue solving to next iteration. + SOLVER_CONTINUE, + + // Terminate solver, and do not update the parameter blocks upon + // return. Unless the user has set + // Solver:Options:::update_state_every_iteration, in which case the + // state would have been updated every iteration + // anyways. Solver::Summary::termination_type is set to USER_ABORT. + SOLVER_ABORT, + + // Terminate solver, update state and + // return. Solver::Summary::termination_type is set to USER_SUCCESS. + SOLVER_TERMINATE_SUCCESSFULLY +}; + +// The format in which linear least squares problems should be logged +// when Solver::Options::lsqp_iterations_to_dump is non-empty. +enum DumpFormatType { + // Print the linear least squares problem in a human readable format + // to stderr. The Jacobian is printed as a dense matrix. The vectors + // D, x and f are printed as dense vectors. This should only be used + // for small problems. + CONSOLE, + + // Write out the linear least squares problem to the directory + // pointed to by Solver::Options::lsqp_dump_directory as a protocol + // buffer. linear_least_squares_problems.h/cc contains routines for + // loading these problems. For details on the on disk format used, + // see matrix.proto. The files are named lm_iteration_???.lsqp. + PROTOBUF, + + // Write out the linear least squares problem to the directory + // pointed to by Solver::Options::lsqp_dump_directory as text files + // which can be read into MATLAB/Octave. The Jacobian is dumped as a + // text file containing (i,j,s) triplets, the vectors D, x and f are + // dumped as text files containing a list of their values. + // + // A MATLAB/octave script called lm_iteration_???.m is also output, + // which can be used to parse and load the problem into memory. + TEXTFILE +}; + +// For SizedCostFunction and AutoDiffCostFunction, DYNAMIC can be specified for +// the number of residuals. If specified, then the number of residuas for that +// cost function can vary at runtime. +enum DimensionType { + DYNAMIC = -1 +}; + +const char* LinearSolverTypeToString(LinearSolverType type); +const char* PreconditionerTypeToString(PreconditionerType type); +const char* LinearSolverTerminationTypeToString( + LinearSolverTerminationType type); +const char* OrderingTypeToString(OrderingType type); +const char* SolverTerminationTypeToString(SolverTerminationType type); + +bool IsSchurType(LinearSolverType type); + +} // namespace ceres + +#endif // CERES_PUBLIC_TYPES_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_evaluate_preparer.cc b/extern/libmv/third_party/ceres/internal/ceres/block_evaluate_preparer.cc new file mode 100644 index 00000000000..05e63eb560b --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_evaluate_preparer.cc @@ -0,0 +1,73 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#include "ceres/block_evaluate_preparer.h" + +#include <vector> +#include "ceres/block_sparse_matrix.h" +#include "ceres/casts.h" +#include "ceres/parameter_block.h" +#include "ceres/residual_block.h" +#include "ceres/sparse_matrix.h" + +namespace ceres { +namespace internal { + +void BlockEvaluatePreparer::Init(int** jacobian_layout) { + jacobian_layout_ = jacobian_layout; +} + +// Point the jacobian blocks directly into the block sparse matrix. +void BlockEvaluatePreparer::Prepare(const ResidualBlock* residual_block, + int residual_block_index, + SparseMatrix* jacobian, + double** jacobians) const { + CHECK(jacobian != NULL); + double* jacobian_values = + down_cast<BlockSparseMatrix*>(jacobian)->mutable_values(); + + const int* jacobian_block_offset = jacobian_layout_[residual_block_index]; + const int num_parameter_blocks = residual_block->NumParameterBlocks(); + for (int j = 0; j < num_parameter_blocks; ++j) { + if (!residual_block->parameter_blocks()[j]->IsConstant()) { + jacobians[j] = jacobian_values + *jacobian_block_offset; + + // The jacobian_block_offset can't be indexed with 'j' since the code + // that creates the layout strips out any blocks for inactive + // parameters. Instead, bump the pointer for active parameters only. + jacobian_block_offset++; + } else { + jacobians[j] = NULL; + } + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_evaluate_preparer.h b/extern/libmv/third_party/ceres/internal/ceres/block_evaluate_preparer.h new file mode 100644 index 00000000000..a7869311e6e --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_evaluate_preparer.h @@ -0,0 +1,67 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// A evaluate preparer which puts jacobian the evaluated jacobian blocks +// directly into their final resting place in an overall block sparse matrix. +// The evaluator takes care to avoid evaluating the jacobian for fixed +// parameters. + +#ifndef CERES_INTERNAL_BLOCK_EVALUATE_PREPARER_H_ +#define CERES_INTERNAL_BLOCK_EVALUATE_PREPARER_H_ + +namespace ceres { +namespace internal { + +class ResidualBlock; +class SparseMatrix; + +class BlockEvaluatePreparer { + public: + // Using Init() instead of a constructor allows for allocating this structure + // with new[]. This is because C++ doesn't allow passing arguments to objects + // constructed with new[] (as opposed to plain 'new'). + void Init(int** jacobian_layout); + + // EvaluatePreparer interface + + // Point the jacobian blocks directly into the block sparse matrix. + void Prepare(const ResidualBlock* residual_block, + int residual_block_index, + SparseMatrix* jacobian, + double** jacobians) const; + + private: + int const* const* jacobian_layout_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_BLOCK_EVALUATE_PREPARER_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_jacobi_preconditioner.cc b/extern/libmv/third_party/ceres/internal/ceres/block_jacobi_preconditioner.cc new file mode 100644 index 00000000000..1a5001f9c71 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_jacobi_preconditioner.cc @@ -0,0 +1,136 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#include "ceres/block_jacobi_preconditioner.h" + +#include "Eigen/Cholesky" +#include "ceres/block_sparse_matrix.h" +#include "ceres/block_structure.h" +#include "ceres/casts.h" +#include "ceres/integral_types.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +BlockJacobiPreconditioner::BlockJacobiPreconditioner( + const LinearOperator& A) + : block_structure_( + *(down_cast<const BlockSparseMatrix*>(&A)->block_structure())), + num_rows_(A.num_rows()) { + // Calculate the amount of storage needed. + int storage_needed = 0; + for (int c = 0; c < block_structure_.cols.size(); ++c) { + int size = block_structure_.cols[c].size; + storage_needed += size * size; + } + + // Size the offsets and storage. + blocks_.resize(block_structure_.cols.size()); + block_storage_.resize(storage_needed); + + // Put pointers to the storage in the offsets. + double* block_cursor = &block_storage_[0]; + for (int c = 0; c < block_structure_.cols.size(); ++c) { + int size = block_structure_.cols[c].size; + blocks_[c] = block_cursor; + block_cursor += size * size; + } +} + +BlockJacobiPreconditioner::~BlockJacobiPreconditioner() { +} + +void BlockJacobiPreconditioner::Update(const LinearOperator& matrix, const double* D) { + const BlockSparseMatrix& A = *(down_cast<const BlockSparseMatrix*>(&matrix)); + const CompressedRowBlockStructure* bs = A.block_structure(); + + // Compute the diagonal blocks by block inner products. + std::fill(block_storage_.begin(), block_storage_.end(), 0.0); + for (int r = 0; r < bs->rows.size(); ++r) { + const int row_block_size = bs->rows[r].block.size; + const vector<Cell>& cells = bs->rows[r].cells; + const double* row_values = A.RowBlockValues(r); + for (int c = 0; c < cells.size(); ++c) { + const int col_block_size = bs->cols[cells[c].block_id].size; + ConstMatrixRef m(row_values + cells[c].position, + row_block_size, + col_block_size); + + MatrixRef(blocks_[cells[c].block_id], + col_block_size, + col_block_size).noalias() += m.transpose() * m; + + // TODO(keir): Figure out when the below expression is actually faster + // than doing the full rank update. The issue is that for smaller sizes, + // the rankUpdate() function is slower than the full product done above. + // + // On the typical bundling problems, the above product is ~5% faster. + // + // MatrixRef(blocks_[cells[c].block_id], + // col_block_size, + // col_block_size).selfadjointView<Eigen::Upper>().rankUpdate(m); + // + } + } + + // Add the diagonal and invert each block. + for (int c = 0; c < bs->cols.size(); ++c) { + const int size = block_structure_.cols[c].size; + const int position = block_structure_.cols[c].position; + MatrixRef block(blocks_[c], size, size); + + if (D != NULL) { + block.diagonal() += ConstVectorRef(D + position, size).array().square().matrix(); + } + + block = block.selfadjointView<Eigen::Upper>() + .ldlt() + .solve(Matrix::Identity(size, size)); + } +} + +void BlockJacobiPreconditioner::RightMultiply(const double* x, double* y) const { + for (int c = 0; c < block_structure_.cols.size(); ++c) { + const int size = block_structure_.cols[c].size; + const int position = block_structure_.cols[c].position; + ConstMatrixRef D(blocks_[c], size, size); + ConstVectorRef x_block(x + position, size); + VectorRef y_block(y + position, size); + y_block += D * x_block; + } +} + +void BlockJacobiPreconditioner::LeftMultiply(const double* x, double* y) const { + RightMultiply(x, y); +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_jacobi_preconditioner.h b/extern/libmv/third_party/ceres/internal/ceres/block_jacobi_preconditioner.h new file mode 100644 index 00000000000..91cfeddb688 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_jacobi_preconditioner.h @@ -0,0 +1,84 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#ifndef CERES_INTERNAL_BLOCK_JACOBI_PRECONDITIONER_H_ +#define CERES_INTERNAL_BLOCK_JACOBI_PRECONDITIONER_H_ + +#include <vector> +#include "ceres/linear_operator.h" + +namespace ceres { +namespace internal { + +class CompressedRowBlockStructure; +class LinearOperator; +class SparseMatrix; + +// A block Jacobi preconditioner. This is intended for use with conjugate +// gradients, or other iterative symmetric solvers. To use the preconditioner, +// create one by passing a BlockSparseMatrix as the linear operator "A" to the +// constructor. This fixes the sparsity pattern to the pattern of the matrix +// A^TA. +// +// Before each use of the preconditioner in a solve with conjugate gradients, +// update the matrix by running Update(A, D). The values of the matrix A are +// inspected to construct the preconditioner. The vector D is applied as the +// D^TD diagonal term. +class BlockJacobiPreconditioner : public LinearOperator { + public: + // A must remain valid while the BlockJacobiPreconditioner is. + BlockJacobiPreconditioner(const LinearOperator& A); + virtual ~BlockJacobiPreconditioner(); + + // Update the preconditioner with the values found in A. The sparsity pattern + // must match that of the A passed to the constructor. D is a vector that + // must have the same number of rows as A, and is applied as a diagonal in + // addition to the block diagonals of A. + void Update(const LinearOperator& A, const double* D); + + // LinearOperator interface. + virtual void RightMultiply(const double* x, double* y) const; + virtual void LeftMultiply(const double* x, double* y) const; + virtual int num_rows() const { return num_rows_; } + virtual int num_cols() const { return num_rows_; } + + private: + std::vector<double*> blocks_; + std::vector<double> block_storage_; + int num_rows_; + + // The block structure of the matrix this preconditioner is for (e.g. J). + const CompressedRowBlockStructure& block_structure_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_BLOCK_JACOBI_PRECONDITIONER_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_jacobian_writer.cc b/extern/libmv/third_party/ceres/internal/ceres/block_jacobian_writer.cc new file mode 100644 index 00000000000..52a58bb43a6 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_jacobian_writer.cc @@ -0,0 +1,209 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#include "ceres/block_jacobian_writer.h" + +#include "ceres/block_evaluate_preparer.h" +#include "ceres/block_sparse_matrix.h" +#include "ceres/parameter_block.h" +#include "ceres/program.h" +#include "ceres/residual_block.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" + +namespace ceres { +namespace internal { +namespace { + +// Given the residual block ordering, build a lookup table to determine which +// per-parameter jacobian goes where in the overall program jacobian. +// +// Since we expect to use a Schur type linear solver to solve the LM step, take +// extra care to place the E blocks and the F blocks contiguously. E blocks are +// the first num_eliminate_blocks parameter blocks as indicated by the parameter +// block ordering. The remaining parameter blocks are the F blocks. +// +// TODO(keir): Consider if we should use a boolean for each parameter block +// instead of num_eliminate_blocks. +void BuildJacobianLayout(const Program& program, + int num_eliminate_blocks, + vector<int*>* jacobian_layout, + vector<int>* jacobian_layout_storage) { + const vector<ResidualBlock*>& residual_blocks = program.residual_blocks(); + + // Iterate over all the active residual blocks and determine how many E blocks + // are there. This will determine where the F blocks start in the jacobian + // matrix. Also compute the number of jacobian blocks. + int f_block_pos = 0; + int num_jacobian_blocks = 0; + for (int i = 0; i < residual_blocks.size(); ++i) { + ResidualBlock* residual_block = residual_blocks[i]; + const int num_residuals = residual_block->NumResiduals(); + const int num_parameter_blocks = residual_block->NumParameterBlocks(); + + // Advance f_block_pos over each E block for this residual. + for (int j = 0; j < num_parameter_blocks; ++j) { + ParameterBlock* parameter_block = residual_block->parameter_blocks()[j]; + if (!parameter_block->IsConstant()) { + // Only count blocks for active parameters. + num_jacobian_blocks++; + if (parameter_block->index() < num_eliminate_blocks) { + f_block_pos += num_residuals * parameter_block->LocalSize(); + } + } + } + } + + // We now know that the E blocks are laid out starting at zero, and the F + // blocks are laid out starting at f_block_pos. Iterate over the residual + // blocks again, and this time fill the jacobian_layout array with the + // position information. + + jacobian_layout->resize(program.NumResidualBlocks()); + jacobian_layout_storage->resize(num_jacobian_blocks); + + int e_block_pos = 0; + int* jacobian_pos = &(*jacobian_layout_storage)[0]; + for (int i = 0; i < residual_blocks.size(); ++i) { + const ResidualBlock* residual_block = residual_blocks[i]; + const int num_residuals = residual_block->NumResiduals(); + const int num_parameter_blocks = residual_block->NumParameterBlocks(); + + (*jacobian_layout)[i] = jacobian_pos; + for (int j = 0; j < num_parameter_blocks; ++j) { + ParameterBlock* parameter_block = residual_block->parameter_blocks()[j]; + const int parameter_block_index = parameter_block->index(); + if (parameter_block->IsConstant()) { + continue; + } + const int jacobian_block_size = + num_residuals * parameter_block->LocalSize(); + if (parameter_block_index < num_eliminate_blocks) { + *jacobian_pos = e_block_pos; + e_block_pos += jacobian_block_size; + } else { + *jacobian_pos = f_block_pos; + f_block_pos += jacobian_block_size; + } + jacobian_pos++; + } + } +} + +} // namespace + +BlockJacobianWriter::BlockJacobianWriter(const Evaluator::Options& options, + Program* program) + : program_(program) { + CHECK_GE(options.num_eliminate_blocks, 0) + << "num_eliminate_blocks must be greater than 0."; + + BuildJacobianLayout(*program, + options.num_eliminate_blocks, + &jacobian_layout_, + &jacobian_layout_storage_); +} + +// Create evaluate prepareres that point directly into the final jacobian. This +// makes the final Write() a nop. +BlockEvaluatePreparer* BlockJacobianWriter::CreateEvaluatePreparers( + int num_threads) { + BlockEvaluatePreparer* preparers = new BlockEvaluatePreparer[num_threads]; + for (int i = 0; i < num_threads; i++) { + preparers[i].Init(&jacobian_layout_[0]); + } + return preparers; +} + +SparseMatrix* BlockJacobianWriter::CreateJacobian() const { + CompressedRowBlockStructure* bs = new CompressedRowBlockStructure; + + const vector<ParameterBlock*>& parameter_blocks = + program_->parameter_blocks(); + + // Construct the column blocks. + bs->cols.resize(parameter_blocks.size()); + for (int i = 0, cursor = 0; i < parameter_blocks.size(); ++i) { + CHECK_NE(parameter_blocks[i]->index(), -1); + CHECK(!parameter_blocks[i]->IsConstant()); + bs->cols[i].size = parameter_blocks[i]->LocalSize(); + bs->cols[i].position = cursor; + cursor += bs->cols[i].size; + } + + // Construct the cells in each row. + const vector<ResidualBlock*>& residual_blocks = + program_->residual_blocks(); + int row_block_position = 0; + bs->rows.resize(residual_blocks.size()); + for (int i = 0; i < residual_blocks.size(); ++i) { + const ResidualBlock* residual_block = residual_blocks[i]; + CompressedRow* row = &bs->rows[i]; + + row->block.size = residual_block->NumResiduals(); + row->block.position = row_block_position; + row_block_position += row->block.size; + + // Size the row by the number of active parameters in this residual. + const int num_parameter_blocks = residual_block->NumParameterBlocks(); + int num_active_parameter_blocks = 0; + for (int j = 0; j < num_parameter_blocks; ++j) { + if (residual_block->parameter_blocks()[j]->index() != -1) { + num_active_parameter_blocks++; + } + } + row->cells.resize(num_active_parameter_blocks); + + // Add layout information for the active parameters in this row. + for (int j = 0, k = 0; j < num_parameter_blocks; ++j) { + const ParameterBlock* parameter_block = + residual_block->parameter_blocks()[j]; + if (!parameter_block->IsConstant()) { + Cell& cell = row->cells[k]; + cell.block_id = parameter_block->index(); + cell.position = jacobian_layout_[i][k]; + + // Only increment k for active parameters, since there is only layout + // information for active parameters. + k++; + } + } + + sort(row->cells.begin(), row->cells.end(), CellLessThan); + } + + BlockSparseMatrix* jacobian = new BlockSparseMatrix(bs); + CHECK_NOTNULL(jacobian); + return jacobian; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_jacobian_writer.h b/extern/libmv/third_party/ceres/internal/ceres/block_jacobian_writer.h new file mode 100644 index 00000000000..140c7211129 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_jacobian_writer.h @@ -0,0 +1,127 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// A jacobian writer that writes to block sparse matrices. The "writer" name is +// misleading, since the Write() operation on the block jacobian writer does not +// write anything. Instead, the Prepare() method on the BlockEvaluatePreparers +// makes a jacobians array which has direct pointers into the block sparse +// jacobian. When the cost function is evaluated, the jacobian blocks get placed +// directly in their final location. + +#ifndef CERES_INTERNAL_BLOCK_JACOBIAN_WRITER_H_ +#define CERES_INTERNAL_BLOCK_JACOBIAN_WRITER_H_ + +#include <vector> +#include "ceres/evaluator.h" +#include "ceres/internal/port.h" + +namespace ceres { +namespace internal { + +class BlockEvaluatePreparer; +class Program; +class SparseMatrix; + +class BlockJacobianWriter { + public: + BlockJacobianWriter(const Evaluator::Options& options, + Program* program); + + // JacobianWriter interface. + + // Create evaluate prepareres that point directly into the final jacobian. + // This makes the final Write() a nop. + BlockEvaluatePreparer* CreateEvaluatePreparers(int num_threads); + + SparseMatrix* CreateJacobian() const; + + void Write(int /* residual_id */, + int /* residual_offset */, + double** /* jacobians */, + SparseMatrix* /* jacobian */) { + // This is a noop since the blocks were written directly into their final + // position by the outside evaluate call, thanks to the jacobians array + // prepared by the BlockEvaluatePreparers. + } + + private: + Program* program_; + + // Stores the position of each residual / parameter jacobian. + // + // The block sparse matrix that this writer writes to is stored as a set of + // contiguos dense blocks, one after each other; see BlockSparseMatrix. The + // "double* values_" member of the block sparse matrix contains all of these + // blocks. Given a pointer to the first element of a block and the size of + // that block, it's possible to write to it. + // + // In the case of a block sparse jacobian, the jacobian writer needs a way to + // find the offset in the values_ array of each residual/parameter jacobian + // block. + // + // That is the purpose of jacobian_layout_. + // + // In particular, jacobian_layout_[i][j] is the offset in the values_ array of + // the derivative of residual block i with respect to the parameter block at + // active argument position j. + // + // The active qualifier means that non-active parameters do not count. Care + // must be taken when indexing into jacobian_layout_ to account for this. + // Consider a single residual example: + // + // r(x, y, z) + // + // with r in R^3, x in R^4, y in R^2, and z in R^5. + // Take y as a constant (non-active) parameter. + // Take r as residual number 0. + // + // In this case, the active arguments are only (x, z), so the active argument + // position for x is 0, and the active argument position for z is 1. This is + // similar to thinking of r as taking only 2 parameters: + // + // r(x, z) + // + // There are only 2 jacobian blocks: dr/dx and dr/dz. jacobian_layout_ would + // have the following contents: + // + // jacobian_layout_[0] = { 0, 12 } + // + // which indicates that dr/dx is located at values_[0], and dr/dz is at + // values_[12]. See BlockEvaluatePreparer::Prepare()'s comments about 'j'. + vector<int*> jacobian_layout_; + + // The pointers in jacobian_layout_ point directly into this vector. + vector<int> jacobian_layout_storage_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_BLOCK_JACOBIAN_WRITER_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_random_access_dense_matrix.cc b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_dense_matrix.cc new file mode 100644 index 00000000000..2afaf5e2ea2 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_dense_matrix.cc @@ -0,0 +1,83 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/block_random_access_dense_matrix.h" + +#include <vector> +#include <glog/logging.h> +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" + +namespace ceres { +namespace internal { + +BlockRandomAccessDenseMatrix::BlockRandomAccessDenseMatrix( + const vector<int>& blocks) { + block_layout_.resize(blocks.size(), 0); + num_rows_ = 0; + for (int i = 0; i < blocks.size(); ++i) { + block_layout_[i] = num_rows_; + num_rows_ += blocks[i]; + } + + values_.reset(new double[num_rows_ * num_rows_]); + CHECK_NOTNULL(values_.get()); + cell_info_.values = values_.get(); + SetZero(); +} + +// Assume that the user does not hold any locks on any cell blocks +// when they are calling SetZero. +BlockRandomAccessDenseMatrix::~BlockRandomAccessDenseMatrix() { +} + +CellInfo* BlockRandomAccessDenseMatrix::GetCell(const int row_block_id, + const int col_block_id, + int* row, + int* col, + int* row_stride, + int* col_stride) { + *row = block_layout_[row_block_id]; + *col = block_layout_[col_block_id]; + *row_stride = num_rows_; + *col_stride = num_rows_; + return &cell_info_; +} + +// Assume that the user does not hold any locks on any cell blocks +// when they are calling SetZero. +void BlockRandomAccessDenseMatrix::SetZero() { + if (num_rows_) { + VectorRef(values_.get(), num_rows_ * num_rows_).setZero(); + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_random_access_dense_matrix.h b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_dense_matrix.h new file mode 100644 index 00000000000..3a0096209f7 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_dense_matrix.h @@ -0,0 +1,98 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_BLOCK_RANDOM_ACCESS_DENSE_MATRIX_H_ +#define CERES_INTERNAL_BLOCK_RANDOM_ACCESS_DENSE_MATRIX_H_ + +#include "ceres/block_random_access_matrix.h" + +#include <vector> + +#include "ceres/internal/macros.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" + +namespace ceres { +namespace internal { + +// A square block random accessible matrix with the same row and +// column block structure. All cells are stored in the same single +// array, so that its also accessible as a dense matrix of size +// num_rows x num_cols. +// +// This class is NOT thread safe. Since all n^2 cells are stored, +// GetCell never returns NULL for any (row_block_id, col_block_id) +// pair. +// +// ReturnCell is a nop. +class BlockRandomAccessDenseMatrix : public BlockRandomAccessMatrix { + public: + // blocks is a vector of block sizes. The resulting matrix has + // blocks.size() * blocks.size() cells. + explicit BlockRandomAccessDenseMatrix(const vector<int>& blocks); + + // The destructor is not thread safe. It assumes that no one is + // modifying any cells when the matrix is being destroyed. + virtual ~BlockRandomAccessDenseMatrix(); + + // BlockRandomAccessMatrix interface. + virtual CellInfo* GetCell(int row_block_id, + int col_block_id, + int* row, + int* col, + int* row_stride, + int* col_stride); + + // This is not a thread safe method, it assumes that no cell is + // locked. + virtual void SetZero(); + + // Since the matrix is square with the same row and column block + // structure, num_rows() = num_cols(). + virtual int num_rows() const { return num_rows_; } + virtual int num_cols() const { return num_rows_; } + + // The underlying matrix storing the cells. + const double* values() const { return values_.get(); } + double* mutable_values() { return values_.get(); } + + private: + CellInfo cell_info_; + int num_rows_; + vector<int> block_layout_; + scoped_array<double> values_; + + DISALLOW_COPY_AND_ASSIGN(BlockRandomAccessDenseMatrix); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_BLOCK_RANDOM_ACCESS_DENSE_MATRIX_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_random_access_matrix.cc b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_matrix.cc new file mode 100644 index 00000000000..58fe4a10de3 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_matrix.cc @@ -0,0 +1,40 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/block_random_access_matrix.h" + +namespace ceres { +namespace internal { + +BlockRandomAccessMatrix::~BlockRandomAccessMatrix() { +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_random_access_matrix.h b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_matrix.h new file mode 100644 index 00000000000..f398af3be87 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_matrix.h @@ -0,0 +1,132 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Interface for matrices that allow block based random access. + +#ifndef CERES_INTERNAL_BLOCK_RANDOM_ACCESS_MATRIX_H_ +#define CERES_INTERNAL_BLOCK_RANDOM_ACCESS_MATRIX_H_ + +#include "ceres/mutex.h" + +namespace ceres { +namespace internal { + +// A matrix implementing the BlockRandomAccessMatrix interface is a +// matrix whose rows and columns are divided into blocks. For example +// the matrix A: +// +// 3 4 5 +// A = 5 [c_11 c_12 c_13] +// 4 [c_21 c_22 c_23] +// +// has row blocks of size 5 and 4, and column blocks of size 3, 4 and +// 5. It has six cells corresponding to the six row-column block +// combinations. +// +// BlockRandomAccessMatrix objects provide access to cells c_ij using +// the GetCell method. when a cell is present, GetCell will return a +// CellInfo object containing a pointer to an array which contains the +// cell as a submatrix and a mutex that guards this submatrix. If the +// user is accessing the matrix concurrently, it is his responsibility +// to use the mutex to exclude other writers from writing to the cell +// concurrently. +// +// There is no requirement that all cells be present, i.e. the matrix +// itself can be block sparse. When a cell is not present, the GetCell +// method will return a NULL pointer. +// +// There is no requirement about how the cells are stored beyond that +// form a dense submatrix of a larger dense matrix. Like everywhere +// else in Ceres, RowMajor storage assumed. +// +// Example usage: +// +// BlockRandomAccessMatrix* A = new BlockRandomAccessMatrixSubClass(...) +// +// int row, col, row_stride, col_stride; +// CellInfo* cell = A->GetCell(row_block_id, col_block_id, +// &row, &col, +// &row_stride, &col_stride); +// +// if (cell != NULL) { +// MatrixRef m(cell->values, row_stride, col_stride); +// MutexLock l(&cell->m); +// m.block(row, col, row_block_size, col_block_size) = ... +// } + +// Structure to carry a pointer to the array containing a cell and the +// Mutex guarding it. +struct CellInfo { + CellInfo() + : values(NULL) { + } + + explicit CellInfo(double* ptr) + : values(ptr) { + } + + double* values; + Mutex m; +}; + +class BlockRandomAccessMatrix { + public: + virtual ~BlockRandomAccessMatrix(); + + // If the cell (row_block_id, col_block_id) is present, then return + // a CellInfo with a pointer to the dense matrix containing it, + // otherwise return NULL. The dense matrix containing this cell has + // size row_stride, col_stride and the cell is located at position + // (row, col) within this matrix. + // + // The size of the cell is row_block_size x col_block_size is + // assumed known to the caller. row_block_size less than or equal to + // row_stride and col_block_size is upper bounded by col_stride. + virtual CellInfo* GetCell(int row_block_id, + int col_block_id, + int* row, + int* col, + int* row_stride, + int* col_stride) = 0; + + // Zero out the values of the array. The structure of the matrix + // (size and sparsity) is preserved. + virtual void SetZero() = 0; + + // Number of scalar rows and columns in the matrix, i.e the sum of + // all row blocks and column block sizes respectively. + virtual int num_rows() const = 0; + virtual int num_cols() const = 0; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_BLOCK_RANDOM_ACCESS_MATRIX_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_random_access_sparse_matrix.cc b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_sparse_matrix.cc new file mode 100644 index 00000000000..c496fcd13de --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_sparse_matrix.cc @@ -0,0 +1,158 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/block_random_access_sparse_matrix.h" + +#include <algorithm> +#include <set> +#include <utility> +#include <vector> +#include <glog/logging.h> +#include "ceres/mutex.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +BlockRandomAccessSparseMatrix::BlockRandomAccessSparseMatrix( + const vector<int>& blocks, + const set<pair<int, int> >& block_pairs) + : kMaxRowBlocks(10 * 1000 * 1000), + blocks_(blocks) { + CHECK_LT(blocks.size(), kMaxRowBlocks); + + // Build the row/column layout vector and count the number of scalar + // rows/columns. + int num_cols = 0; + vector<int> col_layout; + for (int i = 0; i < blocks_.size(); ++i) { + col_layout.push_back(num_cols); + num_cols += blocks_[i]; + } + + // Count the number of scalar non-zero entries and build the layout + // object for looking into the values array of the + // TripletSparseMatrix. + int num_nonzeros = 0; + for (set<pair<int, int> >::const_iterator it = block_pairs.begin(); + it != block_pairs.end(); + ++it) { + const int row_block_size = blocks_[it->first]; + const int col_block_size = blocks_[it->second]; + num_nonzeros += row_block_size * col_block_size; + } + + VLOG(1) << "Matrix Size [" << num_cols + << "," << num_cols + << "] " << num_nonzeros; + + tsm_.reset(new TripletSparseMatrix(num_cols, num_cols, num_nonzeros)); + tsm_->set_num_nonzeros(num_nonzeros); + int* rows = tsm_->mutable_rows(); + int* cols = tsm_->mutable_cols(); + double* values = tsm_->mutable_values(); + + int pos = 0; + for (set<pair<int, int> >::const_iterator it = block_pairs.begin(); + it != block_pairs.end(); + ++it) { + const int row_block_size = blocks_[it->first]; + const int col_block_size = blocks_[it->second]; + layout_[IntPairToLong(it->first, it->second)] = + new CellInfo(values + pos); + pos += row_block_size * col_block_size; + } + + // Fill the sparsity pattern of the underlying matrix. + for (set<pair<int, int> >::const_iterator it = block_pairs.begin(); + it != block_pairs.end(); + ++it) { + const int row_block_id = it->first; + const int col_block_id = it->second; + const int row_block_size = blocks_[row_block_id]; + const int col_block_size = blocks_[col_block_id]; + int pos = + layout_[IntPairToLong(row_block_id, col_block_id)]->values - values; + for (int r = 0; r < row_block_size; ++r) { + for (int c = 0; c < col_block_size; ++c, ++pos) { + rows[pos] = col_layout[row_block_id] + r; + cols[pos] = col_layout[col_block_id] + c; + values[pos] = 1.0; + DCHECK_LT(rows[pos], tsm_->num_rows()); + DCHECK_LT(cols[pos], tsm_->num_rows()); + } + } + } +} + +// Assume that the user does not hold any locks on any cell blocks +// when they are calling SetZero. +BlockRandomAccessSparseMatrix::~BlockRandomAccessSparseMatrix() { + for (LayoutType::iterator it = layout_.begin(); + it != layout_.end(); + ++it) { + delete it->second; + } +} + +CellInfo* BlockRandomAccessSparseMatrix::GetCell(int row_block_id, + int col_block_id, + int* row, + int* col, + int* row_stride, + int* col_stride) { + const LayoutType::iterator it = + layout_.find(IntPairToLong(row_block_id, col_block_id)); + if (it == layout_.end()) { + return NULL; + } + + // Each cell is stored contiguously as its own little dense matrix. + *row = 0; + *col = 0; + *row_stride = blocks_[row_block_id]; + *col_stride = blocks_[col_block_id]; + return it->second; +} + +// Assume that the user does not hold any locks on any cell blocks +// when they are calling SetZero. +void BlockRandomAccessSparseMatrix::SetZero() { + if (tsm_->num_nonzeros()) { + VectorRef(tsm_->mutable_values(), + tsm_->num_nonzeros()).setZero(); + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_random_access_sparse_matrix.h b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_sparse_matrix.h new file mode 100644 index 00000000000..12613c3daa0 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_random_access_sparse_matrix.h @@ -0,0 +1,109 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_BLOCK_RANDOM_ACCESS_SPARSE_MATRIX_H_ +#define CERES_INTERNAL_BLOCK_RANDOM_ACCESS_SPARSE_MATRIX_H_ + +#include <set> +#include <vector> +#include <utility> +#include "ceres/mutex.h" +#include "ceres/block_random_access_matrix.h" +#include "ceres/collections_port.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/internal/macros.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +// A threaf safe square block sparse implementation of +// BlockRandomAccessMatrix. Internally a TripletSparseMatrix is used +// for doing the actual storage. This class augments this matrix with +// an unordered_map that allows random read/write access. +class BlockRandomAccessSparseMatrix : public BlockRandomAccessMatrix { + public: + // blocks is an array of block sizes. block_pairs is a set of + // <row_block_id, col_block_id> pairs to identify the non-zero cells + // of this matrix. + BlockRandomAccessSparseMatrix(const vector<int>& blocks, + const set<pair<int, int> >& block_pairs); + + // The destructor is not thread safe. It assumes that no one is + // modifying any cells when the matrix is being destroyed. + virtual ~BlockRandomAccessSparseMatrix(); + + // BlockRandomAccessMatrix Interface. + virtual CellInfo* GetCell(int row_block_id, + int col_block_id, + int* row, + int* col, + int* row_stride, + int* col_stride); + + // This is not a thread safe method, it assumes that no cell is + // locked. + virtual void SetZero(); + virtual bool IsThreadSafe() const { return true; } + + // Since the matrix is square, num_rows() == num_cols(). + virtual int num_rows() const { return tsm_->num_rows(); } + virtual int num_cols() const { return tsm_->num_cols(); } + + // Access to the underlying matrix object. + const TripletSparseMatrix* matrix() const { return tsm_.get(); } + TripletSparseMatrix* mutable_matrix() { return tsm_.get(); } + + private: + long int IntPairToLong(int a, int b) { + return a * kMaxRowBlocks + b; + } + + const int kMaxRowBlocks; + // row/column block sizes. + const vector<int> blocks_; + + // A mapping from <row_block_id, col_block_id> to the position in + // the values array of tsm_ where the block is stored. + typedef HashMap<long int, CellInfo* > LayoutType; + LayoutType layout_; + + // The underlying matrix object which actually stores the cells. + scoped_ptr<TripletSparseMatrix> tsm_; + + DISALLOW_COPY_AND_ASSIGN(BlockRandomAccessSparseMatrix); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_BLOCK_RANDOM_ACCESS_SPARSE_MATRIX_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_sparse_matrix.cc b/extern/libmv/third_party/ceres/internal/ceres/block_sparse_matrix.cc new file mode 100644 index 00000000000..7dd395e2975 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_sparse_matrix.cc @@ -0,0 +1,286 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/block_sparse_matrix.h" + +#include <cstddef> +#include <algorithm> +#include <vector> +#include <glog/logging.h> +#include "ceres/block_structure.h" +#include "ceres/matrix_proto.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +BlockSparseMatrix::~BlockSparseMatrix() {} + +BlockSparseMatrix::BlockSparseMatrix( + CompressedRowBlockStructure* block_structure) + : num_rows_(0), + num_cols_(0), + num_nonzeros_(0), + values_(NULL), + block_structure_(block_structure) { + CHECK_NOTNULL(block_structure_.get()); + + // Count the number of columns in the matrix. + for (int i = 0; i < block_structure_->cols.size(); ++i) { + num_cols_ += block_structure_->cols[i].size; + } + + // Count the number of non-zero entries and the number of rows in + // the matrix. + for (int i = 0; i < block_structure_->rows.size(); ++i) { + int row_block_size = block_structure_->rows[i].block.size; + num_rows_ += row_block_size; + + const vector<Cell>& cells = block_structure_->rows[i].cells; + for (int j = 0; j < cells.size(); ++j) { + int col_block_id = cells[j].block_id; + int col_block_size = block_structure_->cols[col_block_id].size; + num_nonzeros_ += col_block_size * row_block_size; + } + } + + CHECK_GE(num_rows_, 0); + CHECK_GE(num_cols_, 0); + CHECK_GE(num_nonzeros_, 0); + VLOG(2) << "Allocating values array with " + << num_nonzeros_ * sizeof(double) << " bytes."; // NOLINT + values_.reset(new double[num_nonzeros_]); + CHECK_NOTNULL(values_.get()); +} + +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +BlockSparseMatrix::BlockSparseMatrix(const SparseMatrixProto& outer_proto) { + CHECK(outer_proto.has_block_matrix()); + + const BlockSparseMatrixProto& proto = outer_proto.block_matrix(); + CHECK(proto.has_num_rows()); + CHECK(proto.has_num_cols()); + CHECK_EQ(proto.num_nonzeros(), proto.values_size()); + + num_rows_ = proto.num_rows(); + num_cols_ = proto.num_cols(); + num_nonzeros_ = proto.num_nonzeros(); + + // Copy out the values into *this. + values_.reset(new double[num_nonzeros_]); + for (int i = 0; i < proto.num_nonzeros(); ++i) { + values_[i] = proto.values(i); + } + + // Create the block structure according to the proto. + block_structure_.reset(new CompressedRowBlockStructure); + ProtoToBlockStructure(proto.block_structure(), block_structure_.get()); +} +#endif + +void BlockSparseMatrix::SetZero() { + fill(values_.get(), values_.get() + num_nonzeros_, 0.0); +} + +void BlockSparseMatrix::RightMultiply(const double* x, double* y) const { + CHECK_NOTNULL(x); + CHECK_NOTNULL(y); + + for (int i = 0; i < block_structure_->rows.size(); ++i) { + int row_block_pos = block_structure_->rows[i].block.position; + int row_block_size = block_structure_->rows[i].block.size; + VectorRef yref(y + row_block_pos, row_block_size); + const vector<Cell>& cells = block_structure_->rows[i].cells; + for (int j = 0; j < cells.size(); ++j) { + int col_block_id = cells[j].block_id; + int col_block_size = block_structure_->cols[col_block_id].size; + int col_block_pos = block_structure_->cols[col_block_id].position; + ConstVectorRef xref(x + col_block_pos, col_block_size); + MatrixRef m(values_.get() + cells[j].position, + row_block_size, col_block_size); + yref += m.lazyProduct(xref); + } + } +} + +void BlockSparseMatrix::LeftMultiply(const double* x, double* y) const { + CHECK_NOTNULL(x); + CHECK_NOTNULL(y); + + for (int i = 0; i < block_structure_->rows.size(); ++i) { + int row_block_pos = block_structure_->rows[i].block.position; + int row_block_size = block_structure_->rows[i].block.size; + const ConstVectorRef xref(x + row_block_pos, row_block_size); + const vector<Cell>& cells = block_structure_->rows[i].cells; + for (int j = 0; j < cells.size(); ++j) { + int col_block_id = cells[j].block_id; + int col_block_size = block_structure_->cols[col_block_id].size; + int col_block_pos = block_structure_->cols[col_block_id].position; + VectorRef yref(y + col_block_pos, col_block_size); + MatrixRef m(values_.get() + cells[j].position, + row_block_size, col_block_size); + yref += m.transpose().lazyProduct(xref); + } + } +} + +void BlockSparseMatrix::SquaredColumnNorm(double* x) const { + CHECK_NOTNULL(x); + VectorRef(x, num_cols_).setZero(); + for (int i = 0; i < block_structure_->rows.size(); ++i) { + int row_block_size = block_structure_->rows[i].block.size; + const vector<Cell>& cells = block_structure_->rows[i].cells; + for (int j = 0; j < cells.size(); ++j) { + int col_block_id = cells[j].block_id; + int col_block_size = block_structure_->cols[col_block_id].size; + int col_block_pos = block_structure_->cols[col_block_id].position; + const MatrixRef m(values_.get() + cells[j].position, + row_block_size, col_block_size); + VectorRef(x + col_block_pos, col_block_size) += m.colwise().squaredNorm(); + } + } +} + +void BlockSparseMatrix::ScaleColumns(const double* scale) { + CHECK_NOTNULL(scale); + + for (int i = 0; i < block_structure_->rows.size(); ++i) { + int row_block_size = block_structure_->rows[i].block.size; + const vector<Cell>& cells = block_structure_->rows[i].cells; + for (int j = 0; j < cells.size(); ++j) { + int col_block_id = cells[j].block_id; + int col_block_size = block_structure_->cols[col_block_id].size; + int col_block_pos = block_structure_->cols[col_block_id].position; + MatrixRef m(values_.get() + cells[j].position, + row_block_size, col_block_size); + m *= ConstVectorRef(scale + col_block_pos, col_block_size).asDiagonal(); + } + } +} + +void BlockSparseMatrix::ToDenseMatrix(Matrix* dense_matrix) const { + CHECK_NOTNULL(dense_matrix); + + dense_matrix->resize(num_rows_, num_cols_); + dense_matrix->setZero(); + Matrix& m = *dense_matrix; + + for (int i = 0; i < block_structure_->rows.size(); ++i) { + int row_block_pos = block_structure_->rows[i].block.position; + int row_block_size = block_structure_->rows[i].block.size; + const vector<Cell>& cells = block_structure_->rows[i].cells; + for (int j = 0; j < cells.size(); ++j) { + int col_block_id = cells[j].block_id; + int col_block_size = block_structure_->cols[col_block_id].size; + int col_block_pos = block_structure_->cols[col_block_id].position; + int jac_pos = cells[j].position; + m.block(row_block_pos, col_block_pos, row_block_size, col_block_size) + += MatrixRef(values_.get() + jac_pos, row_block_size, col_block_size); + } + } +} + +void BlockSparseMatrix::ToTripletSparseMatrix( + TripletSparseMatrix* matrix) const { + CHECK_NOTNULL(matrix); + + matrix->Reserve(num_nonzeros_); + matrix->Resize(num_rows_, num_cols_); + matrix->SetZero(); + + for (int i = 0; i < block_structure_->rows.size(); ++i) { + int row_block_pos = block_structure_->rows[i].block.position; + int row_block_size = block_structure_->rows[i].block.size; + const vector<Cell>& cells = block_structure_->rows[i].cells; + for (int j = 0; j < cells.size(); ++j) { + int col_block_id = cells[j].block_id; + int col_block_size = block_structure_->cols[col_block_id].size; + int col_block_pos = block_structure_->cols[col_block_id].position; + int jac_pos = cells[j].position; + for (int r = 0; r < row_block_size; ++r) { + for (int c = 0; c < col_block_size; ++c, ++jac_pos) { + matrix->mutable_rows()[jac_pos] = row_block_pos + r; + matrix->mutable_cols()[jac_pos] = col_block_pos + c; + matrix->mutable_values()[jac_pos] = values_[jac_pos]; + } + } + } + } + matrix->set_num_nonzeros(num_nonzeros_); +} + +// Return a pointer to the block structure. We continue to hold +// ownership of the object though. +const CompressedRowBlockStructure* BlockSparseMatrix::block_structure() + const { + return block_structure_.get(); +} + +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +void BlockSparseMatrix::ToProto(SparseMatrixProto* outer_proto) const { + outer_proto->Clear(); + + BlockSparseMatrixProto* proto = outer_proto->mutable_block_matrix(); + proto->set_num_rows(num_rows_); + proto->set_num_cols(num_cols_); + proto->set_num_nonzeros(num_nonzeros_); + for (int i = 0; i < num_nonzeros_; ++i) { + proto->add_values(values_[i]); + } + BlockStructureToProto(*block_structure_, proto->mutable_block_structure()); +} +#endif + +void BlockSparseMatrix::ToTextFile(FILE* file) const { + CHECK_NOTNULL(file); + for (int i = 0; i < block_structure_->rows.size(); ++i) { + const int row_block_pos = block_structure_->rows[i].block.position; + const int row_block_size = block_structure_->rows[i].block.size; + const vector<Cell>& cells = block_structure_->rows[i].cells; + for (int j = 0; j < cells.size(); ++j) { + const int col_block_id = cells[j].block_id; + const int col_block_size = block_structure_->cols[col_block_id].size; + const int col_block_pos = block_structure_->cols[col_block_id].position; + int jac_pos = cells[j].position; + for (int r = 0; r < row_block_size; ++r) { + for (int c = 0; c < col_block_size; ++c) { + fprintf(file, "% 10d % 10d %17f\n", + row_block_pos + r, + col_block_pos + c, + values_[jac_pos++]); + } + } + } + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_sparse_matrix.h b/extern/libmv/third_party/ceres/internal/ceres/block_sparse_matrix.h new file mode 100644 index 00000000000..f71446e8f58 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_sparse_matrix.h @@ -0,0 +1,144 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Implementation of the SparseMatrix interface for block sparse +// matrices. + +#ifndef CERES_INTERNAL_BLOCK_SPARSE_MATRIX_H_ +#define CERES_INTERNAL_BLOCK_SPARSE_MATRIX_H_ + +#include "ceres/block_structure.h" +#include "ceres/sparse_matrix.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/macros.h" +#include "ceres/internal/scoped_ptr.h" + +namespace ceres { +namespace internal { + +class SparseMatrixProto; +class TripletSparseMatrix; + +// A further extension of the SparseMatrix interface to support block-oriented +// matrices. The key addition is the RowBlockValues() accessor, which enables +// the lazy block sparse matrix implementation. +class BlockSparseMatrixBase : public SparseMatrix { + public: + BlockSparseMatrixBase() {} + virtual ~BlockSparseMatrixBase() {} + + // Convert this matrix into a triplet sparse matrix. + virtual void ToTripletSparseMatrix(TripletSparseMatrix* matrix) const = 0; + + // Returns a pointer to the block structure. Does not transfer + // ownership. + virtual const CompressedRowBlockStructure* block_structure() const = 0; + + // Returns a pointer to a row of the matrix. The returned array is only valid + // until the next call to RowBlockValues. The caller does not own the result. + // + // The returned array is laid out such that cells on the specified row are + // contiguous in the returned array, though neighbouring cells in row order + // may not be contiguous in the row values. The cell values for cell + // (row_block, cell_block) are found at offset + // + // block_structure()->rows[row_block].cells[cell_block].position + // + virtual const double* RowBlockValues(int row_block_index) const = 0; + + private: + DISALLOW_COPY_AND_ASSIGN(BlockSparseMatrixBase); +}; + +// This class implements the SparseMatrix interface for storing and +// manipulating block sparse matrices. The block structure is stored +// in the CompressedRowBlockStructure object and one is needed to +// initialize the matrix. For details on how the blocks structure of +// the matrix is stored please see the documentation +// +// internal/ceres/block_structure.h +// +class BlockSparseMatrix : public BlockSparseMatrixBase { + public: + // Construct a block sparse matrix with a fully initialized + // CompressedRowBlockStructure objected. The matrix takes over + // ownership of this object and destroys it upon destruction. + // + // TODO(sameeragarwal): Add a function which will validate legal + // CompressedRowBlockStructure objects. + explicit BlockSparseMatrix(CompressedRowBlockStructure* block_structure); + + // Construct a block sparse matrix from a protocol buffer. +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS + explicit BlockSparseMatrix(const SparseMatrixProto& proto); +#endif + + BlockSparseMatrix(); + virtual ~BlockSparseMatrix(); + + // Implementation of SparseMatrix interface. + virtual void SetZero(); + virtual void RightMultiply(const double* x, double* y) const; + virtual void LeftMultiply(const double* x, double* y) const; + virtual void SquaredColumnNorm(double* x) const; + virtual void ScaleColumns(const double* scale); + virtual void ToDenseMatrix(Matrix* dense_matrix) const; +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS + virtual void ToProto(SparseMatrixProto* proto) const; +#endif + virtual void ToTextFile(FILE* file) const; + + virtual int num_rows() const { return num_rows_; } + virtual int num_cols() const { return num_cols_; } + virtual int num_nonzeros() const { return num_nonzeros_; } + virtual const double* values() const { return values_.get(); } + virtual double* mutable_values() { return values_.get(); } + + // Implementation of BlockSparseMatrixBase interface. + virtual void ToTripletSparseMatrix(TripletSparseMatrix* matrix) const; + virtual const CompressedRowBlockStructure* block_structure() const; + virtual const double* RowBlockValues(int row_block_index) const { + return values_.get(); + } + + private: + int num_rows_; + int num_cols_; + int max_num_nonzeros_; + int num_nonzeros_; + scoped_array<double> values_; + scoped_ptr<CompressedRowBlockStructure> block_structure_; + DISALLOW_COPY_AND_ASSIGN(BlockSparseMatrix); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_BLOCK_SPARSE_MATRIX_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_structure.cc b/extern/libmv/third_party/ceres/internal/ceres/block_structure.cc new file mode 100644 index 00000000000..5add4f3b94d --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_structure.cc @@ -0,0 +1,92 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/block_structure.h" +#include "ceres/matrix_proto.h" + +namespace ceres { +namespace internal { + +bool CellLessThan(const Cell& lhs, const Cell& rhs) { + return (lhs.block_id < rhs.block_id); +} + +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +void ProtoToBlockStructure(const BlockStructureProto &proto, + CompressedRowBlockStructure *block_structure) { + // Decode the column blocks. + block_structure->cols.resize(proto.cols_size()); + for (int i = 0; i < proto.cols_size(); ++i) { + block_structure->cols[i].size = proto.cols(i).size(); + block_structure->cols[i].position = + proto.cols(i).position(); + } + // Decode the row structure. + block_structure->rows.resize(proto.rows_size()); + for (int i = 0; i < proto.rows_size(); ++i) { + const CompressedRowProto &row = proto.rows(i); + block_structure->rows[i].block.size = row.block().size(); + block_structure->rows[i].block.position = row.block().position(); + + // Copy the cells within the row. + block_structure->rows[i].cells.resize(row.cells_size()); + for (int j = 0; j < row.cells_size(); ++j) { + const CellProto &cell = row.cells(j); + block_structure->rows[i].cells[j].block_id = cell.block_id(); + block_structure->rows[i].cells[j].position = cell.position(); + } + } +} + +void BlockStructureToProto(const CompressedRowBlockStructure &block_structure, + BlockStructureProto *proto) { + // Encode the column blocks. + for (int i = 0; i < block_structure.cols.size(); ++i) { + BlockProto *block = proto->add_cols(); + block->set_size(block_structure.cols[i].size); + block->set_position(block_structure.cols[i].position); + } + // Encode the row structure. + for (int i = 0; i < block_structure.rows.size(); ++i) { + CompressedRowProto *row = proto->add_rows(); + BlockProto *block = row->mutable_block(); + block->set_size(block_structure.rows[i].block.size); + block->set_position(block_structure.rows[i].block.position); + for (int j = 0; j < block_structure.rows[i].cells.size(); ++j) { + CellProto *cell = row->add_cells(); + cell->set_block_id(block_structure.rows[i].cells[j].block_id); + cell->set_position(block_structure.rows[i].cells[j].position); + } + } +} +#endif + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/block_structure.h b/extern/libmv/third_party/ceres/internal/ceres/block_structure.h new file mode 100644 index 00000000000..f509067d216 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/block_structure.h @@ -0,0 +1,105 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Block structure objects are used to carry information about the +// dense block structure of sparse matrices. The BlockSparseMatrix +// object uses the BlockStructure objects to keep track of the matrix +// structure and operate upon it. This allows us to use more cache +// friendly block oriented linear algebra operations on the matrix +// instead of accessing it one scalar entry at a time. + +#ifndef CERES_INTERNAL_BLOCK_STRUCTURE_H_ +#define CERES_INTERNAL_BLOCK_STRUCTURE_H_ + +#include <vector> +#include "ceres/internal/port.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +class BlockStructureProto; + +typedef int16 BlockSize; + +struct Block { + Block() : size(-1), position(-1) {} + Block(int size_, int position_) : size(size_), position(position_) {} + + BlockSize size; + int position; // Position along the row/column. +}; + +struct Cell { + Cell() : block_id(-1), position(-1) {} + Cell(int block_id_, int position_) + : block_id(block_id_), position(position_) {} + + // Column or row block id as the case maybe. + int block_id; + // Where in the values array of the jacobian is this cell located. + int position; +}; + +// Order cell by their block_id; +bool CellLessThan(const Cell& lhs, const Cell& rhs); + +struct CompressedList { + Block block; + vector<Cell> cells; +}; + +typedef CompressedList CompressedRow; +typedef CompressedList CompressedColumn; + +struct CompressedRowBlockStructure { + vector<Block> cols; + vector<CompressedRow> rows; +}; + +struct CompressedColumnBlockStructure { + vector<Block> rows; + vector<CompressedColumn> cols; +}; + +// Deserialize the given block structure proto to the given block structure. +// Destroys previous contents of block_structure. +void ProtoToBlockStructure(const BlockStructureProto &proto, + CompressedRowBlockStructure *block_structure); + +// Serialize the given block structure to the given proto. Destroys previous +// contents of proto. +void BlockStructureToProto(const CompressedRowBlockStructure &block_structure, + BlockStructureProto *proto); + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_BLOCK_STRUCTURE_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/canonical_views_clustering.cc b/extern/libmv/third_party/ceres/internal/ceres/canonical_views_clustering.cc new file mode 100644 index 00000000000..53190ada6fc --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/canonical_views_clustering.cc @@ -0,0 +1,238 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: David Gallup (dgallup@google.com) +// Sameer Agarwal (sameeragarwal@google.com) + +#include "ceres/canonical_views_clustering.h" + +#include <glog/logging.h> +#include "ceres/graph.h" +#include "ceres/collections_port.h" +#include "ceres/map_util.h" +#include "ceres/internal/macros.h" + +namespace ceres { +namespace internal { + +typedef HashMap<int, int> IntMap; +typedef HashSet<int> IntSet; + +class CanonicalViewsClustering { + public: + CanonicalViewsClustering() {} + + // Compute the canonical views clustering of the vertices of the + // graph. centers will contain the vertices that are the identified + // as the canonical views/cluster centers, and membership is a map + // from vertices to cluster_ids. The i^th cluster center corresponds + // to the i^th cluster. It is possible depending on the + // configuration of the clustering algorithm that some of the + // vertices may not be assigned to any cluster. In this case they + // are assigned to a cluster with id = kInvalidClusterId. + void ComputeClustering(const Graph<int>& graph, + const CanonicalViewsClusteringOptions& options, + vector<int>* centers, + IntMap* membership); + + private: + void FindValidViews(IntSet* valid_views) const; + double ComputeClusteringQualityDifference(const int candidate, + const vector<int>& centers) const; + void UpdateCanonicalViewAssignments(const int canonical_view); + void ComputeClusterMembership(const vector<int>& centers, + IntMap* membership) const; + + CanonicalViewsClusteringOptions options_; + const Graph<int>* graph_; + // Maps a view to its representative canonical view (its cluster + // center). + IntMap view_to_canonical_view_; + // Maps a view to its similarity to its current cluster center. + HashMap<int, double> view_to_canonical_view_similarity_; + DISALLOW_COPY_AND_ASSIGN(CanonicalViewsClustering); +}; + +void ComputeCanonicalViewsClustering( + const Graph<int>& graph, + const CanonicalViewsClusteringOptions& options, + vector<int>* centers, + IntMap* membership) { + time_t start_time = time(NULL); + CanonicalViewsClustering cv; + cv.ComputeClustering(graph, options, centers, membership); + VLOG(2) << "Canonical views clustering time (secs): " + << time(NULL) - start_time; +} + +// Implementation of CanonicalViewsClustering +void CanonicalViewsClustering::ComputeClustering( + const Graph<int>& graph, + const CanonicalViewsClusteringOptions& options, + vector<int>* centers, + IntMap* membership) { + options_ = options; + CHECK_NOTNULL(centers)->clear(); + CHECK_NOTNULL(membership)->clear(); + graph_ = &graph; + + IntSet valid_views; + FindValidViews(&valid_views); + while (valid_views.size() > 0) { + // Find the next best canonical view. + double best_difference = -std::numeric_limits<double>::max(); + int best_view = 0; + + // TODO(sameeragarwal): Make this loop multi-threaded. + for (IntSet::const_iterator view = valid_views.begin(); + view != valid_views.end(); + ++view) { + const double difference = + ComputeClusteringQualityDifference(*view, *centers); + if (difference > best_difference) { + best_difference = difference; + best_view = *view; + } + } + + CHECK_GT(best_difference, -std::numeric_limits<double>::max()); + + // Add canonical view if quality improves, or if minimum is not + // yet met, otherwise break. + if ((best_difference <= 0) && + (centers->size() >= options_.min_views)) { + break; + } + + centers->push_back(best_view); + valid_views.erase(best_view); + UpdateCanonicalViewAssignments(best_view); + } + + ComputeClusterMembership(*centers, membership); +} + +// Return the set of vertices of the graph which have valid vertex +// weights. +void CanonicalViewsClustering::FindValidViews( + IntSet* valid_views) const { + const IntSet& views = graph_->vertices(); + for (IntSet::const_iterator view = views.begin(); + view != views.end(); + ++view) { + if (graph_->VertexWeight(*view) != Graph<int>::InvalidWeight()) { + valid_views->insert(*view); + } + } +} + +// Computes the difference in the quality score if 'candidate' were +// added to the set of canonical views. +double CanonicalViewsClustering::ComputeClusteringQualityDifference( + const int candidate, + const vector<int>& centers) const { + // View score. + double difference = + options_.view_score_weight * graph_->VertexWeight(candidate); + + // Compute how much the quality score changes if the candidate view + // was added to the list of canonical views and its nearest + // neighbors became members of its cluster. + const IntSet& neighbors = graph_->Neighbors(candidate); + for (IntSet::const_iterator neighbor = neighbors.begin(); + neighbor != neighbors.end(); + ++neighbor) { + const double old_similarity = + FindWithDefault(view_to_canonical_view_similarity_, *neighbor, 0.0); + const double new_similarity = graph_->EdgeWeight(*neighbor, candidate); + if (new_similarity > old_similarity) { + difference += new_similarity - old_similarity; + } + } + + // Number of views penalty. + difference -= options_.size_penalty_weight; + + // Orthogonality. + for (int i = 0; i < centers.size(); ++i) { + difference -= options_.similarity_penalty_weight * + graph_->EdgeWeight(centers[i], candidate); + } + + return difference; +} + +// Reassign views if they're more similar to the new canonical view. +void CanonicalViewsClustering::UpdateCanonicalViewAssignments( + const int canonical_view) { + const IntSet& neighbors = graph_->Neighbors(canonical_view); + for (IntSet::const_iterator neighbor = neighbors.begin(); + neighbor != neighbors.end(); + ++neighbor) { + const double old_similarity = + FindWithDefault(view_to_canonical_view_similarity_, *neighbor, 0.0); + const double new_similarity = + graph_->EdgeWeight(*neighbor, canonical_view); + if (new_similarity > old_similarity) { + view_to_canonical_view_[*neighbor] = canonical_view; + view_to_canonical_view_similarity_[*neighbor] = new_similarity; + } + } +} + +// Assign a cluster id to each view. +void CanonicalViewsClustering::ComputeClusterMembership( + const vector<int>& centers, + IntMap* membership) const { + CHECK_NOTNULL(membership)->clear(); + + // The i^th cluster has cluster id i. + IntMap center_to_cluster_id; + for (int i = 0; i < centers.size(); ++i) { + center_to_cluster_id[centers[i]] = i; + } + + static const int kInvalidClusterId = -1; + + const IntSet& views = graph_->vertices(); + for (IntSet::const_iterator view = views.begin(); + view != views.end(); + ++view) { + IntMap::const_iterator it = + view_to_canonical_view_.find(*view); + int cluster_id = kInvalidClusterId; + if (it != view_to_canonical_view_.end()) { + cluster_id = FindOrDie(center_to_cluster_id, it->second); + } + + InsertOrDie(membership, *view, cluster_id); + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/canonical_views_clustering.h b/extern/libmv/third_party/ceres/internal/ceres/canonical_views_clustering.h new file mode 100644 index 00000000000..2d1eb403995 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/canonical_views_clustering.h @@ -0,0 +1,133 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// An implementation of the Canonical Views clustering algorithm from +// "Scene Summarization for Online Image Collections", Ian Simon, Noah +// Snavely, Steven M. Seitz, ICCV 2007. +// +// More details can be found at +// http://grail.cs.washington.edu/projects/canonview/ +// +// Ceres uses this algorithm to perform view clustering for +// constructing visibility based preconditioners. + +#ifndef CERES_INTERNAL_CANONICAL_VIEWS_CLUSTERING_H_ +#define CERES_INTERNAL_CANONICAL_VIEWS_CLUSTERING_H_ + +#include <vector> + +#include <glog/logging.h> +#include "ceres/collections_port.h" +#include "ceres/graph.h" +#include "ceres/map_util.h" +#include "ceres/internal/macros.h" + +namespace ceres { +namespace internal { + +class CanonicalViewsClusteringOptions; + +// Compute a partitioning of the vertices of the graph using the +// canonical views clustering algorithm. +// +// In the following we will use the terms vertices and views +// interchangably. Given a weighted Graph G(V,E), the canonical views +// of G are the the set of vertices that best "summarize" the content +// of the graph. If w_ij i s the weight connecting the vertex i to +// vertex j, and C is the set of canonical views. Then the objective +// of the canonical views algorithm is +// +// E[C] = sum_[i in V] max_[j in C] w_ij +// - size_penalty_weight * |C| +// - similarity_penalty_weight * sum_[i in C, j in C, j > i] w_ij +// +// alpha is the size penalty that penalizes large number of canonical +// views. +// +// beta is the similarity penalty that penalizes canonical views that +// are too similar to other canonical views. +// +// Thus the canonical views algorithm tries to find a canonical view +// for each vertex in the graph which best explains it, while trying +// to minimize the number of canonical views and the overlap between +// them. +// +// We further augment the above objective function by allowing for per +// vertex weights, higher weights indicating a higher preference for +// being chosen as a canonical view. Thus if w_i is the vertex weight +// for vertex i, the objective function is then +// +// E[C] = sum_[i in V] max_[j in C] w_ij +// - size_penalty_weight * |C| +// - similarity_penalty_weight * sum_[i in C, j in C, j > i] w_ij +// + view_score_weight * sum_[i in C] w_i +// +// centers will contain the vertices that are the identified +// as the canonical views/cluster centers, and membership is a map +// from vertices to cluster_ids. The i^th cluster center corresponds +// to the i^th cluster. +// +// It is possible depending on the configuration of the clustering +// algorithm that some of the vertices may not be assigned to any +// cluster. In this case they are assigned to a cluster with id = -1; +void ComputeCanonicalViewsClustering( + const Graph<int>& graph, + const CanonicalViewsClusteringOptions& options, + vector<int>* centers, + HashMap<int, int>* membership); + +struct CanonicalViewsClusteringOptions { + CanonicalViewsClusteringOptions() + : min_views(3), + size_penalty_weight(5.75), + similarity_penalty_weight(100.0), + view_score_weight(0.0) { + } + // The minimum number of canonical views to compute. + int min_views; + + // Penalty weight for the number of canonical views. A higher + // number will result in fewer canonical views. + double size_penalty_weight; + + // Penalty weight for the diversity (orthogonality) of the + // canonical views. A higher number will encourage less similar + // canonical views. + double similarity_penalty_weight; + + // Weight for per-view scores. Lower weight places less + // confidence in the view scores. + double view_score_weight; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_CANONICAL_VIEWS_CLUSTERING_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/casts.h b/extern/libmv/third_party/ceres/internal/ceres/casts.h new file mode 100644 index 00000000000..99cf2186cc7 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/casts.h @@ -0,0 +1,108 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#ifndef CERES_INTERNAL_CASTS_H_ +#define CERES_INTERNAL_CASTS_H_ + +#include <cassert> +#include <cstddef> // For NULL. + +namespace ceres { + +// Identity metafunction. +template <class T> +struct identity_ { + typedef T type; +}; + +// Use implicit_cast as a safe version of static_cast or const_cast +// for implicit conversions. For example: +// - Upcasting in a type hierarchy. +// - Performing arithmetic conversions (int32 to int64, int to double, etc.). +// - Adding const or volatile qualifiers. +// +// In general, implicit_cast can be used to convert this code +// To to = from; +// DoSomething(to); +// to this +// DoSomething(implicit_cast<To>(from)); +// +// base::identity_ is used to make a non-deduced context, which +// forces all callers to explicitly specify the template argument. +template<typename To> +inline To implicit_cast(typename identity_<To>::type to) { + return to; +} + +// This version of implicit_cast is used when two template arguments +// are specified. It's obsolete and should not be used. +template<typename To, typename From> +inline To implicit_cast(typename identity_<From>::type const &f) { + return f; +} + +// When you upcast (that is, cast a pointer from type Foo to type +// SuperclassOfFoo), it's fine to use implicit_cast<>, since upcasts +// always succeed. When you downcast (that is, cast a pointer from +// type Foo to type SubclassOfFoo), static_cast<> isn't safe, because +// how do you know the pointer is really of type SubclassOfFoo? It +// could be a bare Foo, or of type DifferentSubclassOfFoo. Thus, +// when you downcast, you should use this macro. In debug mode, we +// use dynamic_cast<> to double-check the downcast is legal (we die +// if it's not). In normal mode, we do the efficient static_cast<> +// instead. Thus, it's important to test in debug mode to make sure +// the cast is legal! +// This is the only place in the code we should use dynamic_cast<>. +// In particular, you SHOULDN'T be using dynamic_cast<> in order to +// do RTTI (eg code like this: +// if (dynamic_cast<Subclass1>(foo)) HandleASubclass1Object(foo); +// if (dynamic_cast<Subclass2>(foo)) HandleASubclass2Object(foo); +// You should design the code some other way not to need this. + +template<typename To, typename From> // use like this: down_cast<T*>(foo); +inline To down_cast(From* f) { // so we only accept pointers + // Ensures that To is a sub-type of From *. This test is here only + // for compile-time type checking, and has no overhead in an + // optimized build at run-time, as it will be optimized away + // completely. + + // TODO(csilvers): This should use COMPILE_ASSERT. + if (false) { + implicit_cast<From*, To>(NULL); + } + + // uses RTTI in dbg and fastbuild. asserts are disabled in opt builds. + assert(f == NULL || dynamic_cast<To>(f) != NULL); // NOLINT + return static_cast<To>(f); +} + +} // namespace ceres + +#endif // CERES_INTERNAL_CASTS_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/cgnr_linear_operator.h b/extern/libmv/third_party/ceres/internal/ceres/cgnr_linear_operator.h new file mode 100644 index 00000000000..f32d8d95c19 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/cgnr_linear_operator.h @@ -0,0 +1,120 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#ifndef CERES_INTERNAL_CGNR_LINEAR_OPERATOR_H_ +#define CERES_INTERNAL_CGNR_LINEAR_OPERATOR_H_ + +#include <algorithm> +#include "ceres/linear_operator.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +class SparseMatrix; + +// A linear operator which takes a matrix A and a diagonal vector D and +// performs products of the form +// +// (A^T A + D^T D)x +// +// This is used to implement iterative general sparse linear solving with +// conjugate gradients, where A is the Jacobian and D is a regularizing +// parameter. A brief proof that D^T D is the correct regularizer: +// +// Given a regularized least squares problem: +// +// min ||Ax - b||^2 + ||Dx||^2 +// x +// +// First expand into matrix notation: +// +// (Ax - b)^T (Ax - b) + xD^TDx +// +// Then multiply out to get: +// +// = xA^TAx - 2b^T Ax + b^Tb + xD^TDx +// +// Take the derivative: +// +// 0 = 2A^TAx - 2A^T b + 2 D^TDx +// 0 = A^TAx - A^T b + D^TDx +// 0 = (A^TA + D^TD)x - A^T b +// +// Thus, the symmetric system we need to solve for CGNR is +// +// Sx = z +// +// with S = A^TA + D^TD +// and z = A^T b +// +// Note: This class is not thread safe, since it uses some temporary storage. +class CgnrLinearOperator : public LinearOperator { + public: + CgnrLinearOperator(const LinearOperator& A, const double *D) + : A_(A), D_(D), z_(new double[A.num_rows()]) { + } + virtual ~CgnrLinearOperator() {} + + virtual void RightMultiply(const double* x, double* y) const { + std::fill(z_.get(), z_.get() + A_.num_rows(), 0.0); + + // z = Ax + A_.RightMultiply(x, z_.get()); + + // y = y + Atz + A_.LeftMultiply(z_.get(), y); + + // y = y + DtDx + if (D_ != NULL) { + int n = A_.num_cols(); + VectorRef(y, n).array() += ConstVectorRef(D_, n).array().square() * + ConstVectorRef(x, n).array(); + } + } + + virtual void LeftMultiply(const double* x, double* y) const { + RightMultiply(x, y); + } + + virtual int num_rows() const { return A_.num_cols(); } + virtual int num_cols() const { return A_.num_cols(); } + + private: + const LinearOperator& A_; + const double* D_; + scoped_array<double> z_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_CGNR_LINEAR_OPERATOR_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/cgnr_solver.cc b/extern/libmv/third_party/ceres/internal/ceres/cgnr_solver.cc new file mode 100644 index 00000000000..ccc8026f9f7 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/cgnr_solver.cc @@ -0,0 +1,80 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#include "ceres/cgnr_solver.h" + +#include "glog/logging.h" +#include "ceres/linear_solver.h" +#include "ceres/cgnr_linear_operator.h" +#include "ceres/conjugate_gradients_solver.h" +#include "ceres/block_jacobi_preconditioner.h" + +namespace ceres { +namespace internal { + +CgnrSolver::CgnrSolver(const LinearSolver::Options& options) + : options_(options), + jacobi_preconditioner_(NULL) { +} + +LinearSolver::Summary CgnrSolver::Solve( + LinearOperator* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x) { + // Form z = Atb. + scoped_array<double> z(new double[A->num_cols()]); + std::fill(z.get(), z.get() + A->num_cols(), 0.0); + A->LeftMultiply(b, z.get()); + + // Precondition if necessary. + LinearSolver::PerSolveOptions cg_per_solve_options = per_solve_options; + if (options_.preconditioner_type == JACOBI) { + if (jacobi_preconditioner_.get() == NULL) { + jacobi_preconditioner_.reset(new BlockJacobiPreconditioner(*A)); + } + jacobi_preconditioner_->Update(*A, per_solve_options.D); + cg_per_solve_options.preconditioner = jacobi_preconditioner_.get(); + } else if (options_.preconditioner_type != IDENTITY) { + LOG(FATAL) << "CGNR only supports IDENTITY and JACOBI preconditioners."; + } + + // Solve (AtA + DtD)x = z (= Atb). + std::fill(x, x + A->num_cols(), 0.0); + CgnrLinearOperator lhs(*A, per_solve_options.D); + ConjugateGradientsSolver conjugate_gradient_solver(options_); + return conjugate_gradient_solver.Solve(&lhs, + z.get(), + cg_per_solve_options, + x); +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/cgnr_solver.h b/extern/libmv/third_party/ceres/internal/ceres/cgnr_solver.h new file mode 100644 index 00000000000..dd36f99006b --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/cgnr_solver.h @@ -0,0 +1,66 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#ifndef CERES_INTERNAL_CGNR_SOLVER_H_ +#define CERES_INTERNAL_CGNR_SOLVER_H_ + +#include "ceres/internal/scoped_ptr.h" +#include "ceres/linear_solver.h" + +namespace ceres { +namespace internal { + +class BlockJacobiPreconditioner; + +// A conjugate gradients on the normal equations solver. This directly solves +// for the solution to +// +// (A^T A + D^T D)x = A^T b +// +// as required for solving for x in the least squares sense. Currently only +// block diagonal preconditioning is supported. +class CgnrSolver : public LinearSolver { + public: + explicit CgnrSolver(const LinearSolver::Options& options); + virtual Summary Solve(LinearOperator* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x); + + private: + const LinearSolver::Options options_; + scoped_ptr<BlockJacobiPreconditioner> jacobi_preconditioner_; + DISALLOW_COPY_AND_ASSIGN(CgnrSolver); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_CGNR_SOLVER_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/collections_port.h b/extern/libmv/third_party/ceres/internal/ceres/collections_port.h new file mode 100644 index 00000000000..e125f3fffcd --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/collections_port.h @@ -0,0 +1,141 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// Portable HashMap and HashSet, and a specialized overload for hashing pairs. + +#ifndef CERES_INTERNAL_COLLECTIONS_PORT_H_ +#define CERES_INTERNAL_COLLECTIONS_PORT_H_ + +#if defined(_MSC_VER) && _MSC_VER <= 1600 +#include <unordered_map> +#include <unordered_set> +#else +#include <tr1/unordered_map> +#include <tr1/unordered_set> +#endif +#include <utility> +#include "ceres/integral_types.h" +#include "ceres/internal/port.h" + +namespace ceres { +namespace internal { + +template<typename K, typename V> +struct HashMap : tr1::unordered_map<K, V> {}; + +template<typename K> +struct HashSet : tr1::unordered_set<K> {}; + +#ifdef _WIN32 +#define GG_LONGLONG(x) x##I64 +#define GG_ULONGLONG(x) x##UI64 +#else +#define GG_LONGLONG(x) x##LL +#define GG_ULONGLONG(x) x##ULL +#endif + +// The hash function is due to Bob Jenkins (see +// http://burtleburtle.net/bob/hash/index.html). Each mix takes 36 instructions, +// in 18 cycles if you're lucky. On x86 architectures, this requires 45 +// instructions in 27 cycles, if you're lucky. +// +// 32bit version +inline void hash_mix(uint32& a, uint32& b, uint32& c) { + a -= b; a -= c; a ^= (c>>13); + b -= c; b -= a; b ^= (a<<8); + c -= a; c -= b; c ^= (b>>13); + a -= b; a -= c; a ^= (c>>12); + b -= c; b -= a; b ^= (a<<16); + c -= a; c -= b; c ^= (b>>5); + a -= b; a -= c; a ^= (c>>3); + b -= c; b -= a; b ^= (a<<10); + c -= a; c -= b; c ^= (b>>15); +} + +// 64bit version +inline void hash_mix(uint64& a, uint64& b, uint64& c) { + a -= b; a -= c; a ^= (c>>43); + b -= c; b -= a; b ^= (a<<9); + c -= a; c -= b; c ^= (b>>8); + a -= b; a -= c; a ^= (c>>38); + b -= c; b -= a; b ^= (a<<23); + c -= a; c -= b; c ^= (b>>5); + a -= b; a -= c; a ^= (c>>35); + b -= c; b -= a; b ^= (a<<49); + c -= a; c -= b; c ^= (b>>11); +} + +inline uint32 Hash32NumWithSeed(uint32 num, uint32 c) { + // The golden ratio; an arbitrary value. + uint32 b = 0x9e3779b9UL; + hash_mix(num, b, c); + return c; +} + +inline uint64 Hash64NumWithSeed(uint64 num, uint64 c) { + // More of the golden ratio. + uint64 b = GG_ULONGLONG(0xe08c1d668b756f82); + hash_mix(num, b, c); + return c; +} + +} // namespace internal +} // namespace ceres + +// Since on some platforms this is a doubly-nested namespace (std::tr1) and +// others it is not, the entire namespace line must be in a macro. +CERES_HASH_NAMESPACE_START + +// The outrageously annoying specializations below are for portability reasons. +// In short, it's not possible to have two overloads of hash<pair<T1, T2> + +// Hasher for STL pairs. Requires hashers for both members to be defined. +template<typename T> +struct hash<pair<T, T> > { + size_t operator()(const pair<T, T>& p) const { + size_t h1 = hash<T>()(p.first); + size_t h2 = hash<T>()(p.second); + // The decision below is at compile time + return (sizeof(h1) <= sizeof(ceres::internal::uint32)) ? + ceres::internal::Hash32NumWithSeed(h1, h2) : + ceres::internal::Hash64NumWithSeed(h1, h2); + } + // Less than operator for MSVC. + bool operator()(const pair<T, T>& a, + const pair<T, T>& b) const { + return a < b; + } + static const size_t bucket_size = 4; // These are required by MSVC + static const size_t min_buckets = 8; // 4 and 8 are defaults. +}; + +CERES_HASH_NAMESPACE_END + +#endif // CERES_INTERNAL_COLLECTIONS_PORT_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/compressed_row_jacobian_writer.cc b/extern/libmv/third_party/ceres/internal/ceres/compressed_row_jacobian_writer.cc new file mode 100644 index 00000000000..aa883b7d353 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/compressed_row_jacobian_writer.cc @@ -0,0 +1,201 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#include "ceres/compressed_row_jacobian_writer.h" + +#include "ceres/casts.h" +#include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/parameter_block.h" +#include "ceres/program.h" +#include "ceres/residual_block.h" +#include "ceres/scratch_evaluate_preparer.h" + +namespace ceres { +namespace internal { + +SparseMatrix* CompressedRowJacobianWriter::CreateJacobian() const { + const vector<ResidualBlock*>& residual_blocks = + program_->residual_blocks(); + + int total_num_residuals = program_->NumResiduals(); + int total_num_effective_parameters = program_->NumEffectiveParameters(); + + // Count the number of jacobian nonzeros. + int num_jacobian_nonzeros = 0; + for (int i = 0; i < residual_blocks.size(); ++i) { + ResidualBlock* residual_block = residual_blocks[i]; + const int num_residuals = residual_block->NumResiduals(); + const int num_parameter_blocks = residual_block->NumParameterBlocks(); + for (int j = 0; j < num_parameter_blocks; ++j) { + ParameterBlock* parameter_block = residual_block->parameter_blocks()[j]; + if (!parameter_block->IsConstant()) { + num_jacobian_nonzeros += num_residuals * parameter_block->LocalSize(); + } + } + } + + // Allocate storage for the jacobian with some extra space at the end. + // Allocate more space than needed to store the jacobian so that when the LM + // algorithm adds the diagonal, no reallocation is necessary. This reduces + // peak memory usage significantly. + CompressedRowSparseMatrix* jacobian = + new CompressedRowSparseMatrix( + total_num_residuals, + total_num_effective_parameters, + num_jacobian_nonzeros + total_num_effective_parameters); + + // At this stage, the CompressedSparseMatrix is an invalid state. But this + // seems to be the only way to construct it without doing a memory copy. + int* rows = jacobian->mutable_rows(); + int* cols = jacobian->mutable_cols(); + int row_pos = 0; + rows[0] = 0; + for (int i = 0; i < residual_blocks.size(); ++i) { + const ResidualBlock* residual_block = residual_blocks[i]; + const int num_parameter_blocks = residual_block->NumParameterBlocks(); + + // Count the number of derivatives for a row of this residual block and + // build a list of active parameter block indices. + int num_derivatives = 0; + vector<int> parameter_indices; + for (int j = 0; j < num_parameter_blocks; ++j) { + ParameterBlock* parameter_block = residual_block->parameter_blocks()[j]; + if (!parameter_block->IsConstant()) { + parameter_indices.push_back(parameter_block->index()); + num_derivatives += parameter_block->LocalSize(); + } + } + + // Sort the parameters by their position in the state vector. + sort(parameter_indices.begin(), parameter_indices.end()); + CHECK(unique(parameter_indices.begin(), parameter_indices.end()) == + parameter_indices.end()) + << "Ceres internal error: " + << "Duplicate parameter blocks detected in a cost function. " + << "This should never happen. Please report this to " + << "the Ceres developers."; + + // Update the row indices. + const int num_residuals = residual_block->NumResiduals(); + for (int j = 0; j < num_residuals; ++j) { + rows[row_pos + j + 1] = rows[row_pos + j] + num_derivatives; + } + + // Iterate over parameter blocks in the order which they occur in the + // parameter vector. This code mirrors that in Write(), where jacobian + // values are updated. + int col_pos = 0; + for (int j = 0; j < parameter_indices.size(); ++j) { + ParameterBlock* parameter_block = + program_->parameter_blocks()[parameter_indices[j]]; + const int parameter_block_size = parameter_block->LocalSize(); + + for (int r = 0; r < num_residuals; ++r) { + // This is the position in the values array of the jacobian where this + // row of the jacobian block should go. + const int column_block_begin = rows[row_pos + r] + col_pos; + + for (int c = 0; c < parameter_block_size; ++c) { + cols[column_block_begin + c] = parameter_block->delta_offset() + c; + } + } + col_pos += parameter_block_size; + } + row_pos += num_residuals; + } + + CHECK_EQ(num_jacobian_nonzeros, rows[total_num_residuals]); + return jacobian; +} + +void CompressedRowJacobianWriter::Write(int residual_id, + int residual_offset, + double **jacobians, + SparseMatrix* base_jacobian) { + CompressedRowSparseMatrix* jacobian = + down_cast<CompressedRowSparseMatrix*>(base_jacobian); + + double* jacobian_values = jacobian->mutable_values(); + const int* jacobian_rows = jacobian->rows(); + + const ResidualBlock* residual_block = + program_->residual_blocks()[residual_id]; + const int num_parameter_blocks = residual_block->NumParameterBlocks(); + const int num_residuals = residual_block->NumResiduals(); + + // It is necessary to determine the order of the jacobian blocks before + // copying them into the CompressedRowSparseMatrix. Just because a cost + // function uses parameter blocks 1 after 2 in its arguments does not mean + // that the block 1 occurs before block 2 in the column layout of the + // jacobian. Thus, determine the order by sorting the jacobian blocks by their + // position in the state vector. + vector<pair<int, int> > evaluated_jacobian_blocks; + for (int j = 0; j < num_parameter_blocks; ++j) { + const ParameterBlock* parameter_block = + residual_block->parameter_blocks()[j]; + if (!parameter_block->IsConstant()) { + evaluated_jacobian_blocks.push_back( + make_pair(parameter_block->index(), j)); + } + } + sort(evaluated_jacobian_blocks.begin(), evaluated_jacobian_blocks.end()); + + // Where in the current row does the jacobian for a parameter block begin. + int col_pos = 0; + + // Iterate over the jacobian blocks in increasing order of their + // positions in the reduced parameter vector. + for (int i = 0; i < evaluated_jacobian_blocks.size(); ++i) { + const ParameterBlock* parameter_block = + program_->parameter_blocks()[evaluated_jacobian_blocks[i].first]; + const int argument = evaluated_jacobian_blocks[i].second; + const int parameter_block_size = parameter_block->LocalSize(); + + // Copy one row of the jacobian block at a time. + for (int r = 0; r < num_residuals; ++r) { + // Position of the r^th row of the current jacobian block. + const double* block_row_begin = + jacobians[argument] + r * parameter_block_size; + + // Position in the values array of the jacobian where this + // row of the jacobian block should go. + double* column_block_begin = + jacobian_values + jacobian_rows[residual_offset + r] + col_pos; + + copy(block_row_begin, + block_row_begin + parameter_block_size, + column_block_begin); + } + col_pos += parameter_block_size; + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/compressed_row_jacobian_writer.h b/extern/libmv/third_party/ceres/internal/ceres/compressed_row_jacobian_writer.h new file mode 100644 index 00000000000..c103165eaf1 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/compressed_row_jacobian_writer.h @@ -0,0 +1,75 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// A jacobian writer that directly writes to compressed row sparse matrices. + +#ifndef CERES_INTERNAL_COMPRESSED_ROW_JACOBIAN_WRITER_H_ +#define CERES_INTERNAL_COMPRESSED_ROW_JACOBIAN_WRITER_H_ + +#include "ceres/evaluator.h" +#include "ceres/scratch_evaluate_preparer.h" + +namespace ceres { +namespace internal { + +class Program; +class SparseMatrix; + +class CompressedRowJacobianWriter { + public: + CompressedRowJacobianWriter(Evaluator::Options /* ignored */, + Program* program) + : program_(program) { + } + + // JacobianWriter interface. + + // Since the compressed row matrix has different layout than that assumed by + // the cost functions, use scratch space to store the jacobians temporarily + // then copy them over to the larger jacobian in the Write() function. + ScratchEvaluatePreparer* CreateEvaluatePreparers(int num_threads) { + return ScratchEvaluatePreparer::Create(*program_, num_threads); + } + + SparseMatrix* CreateJacobian() const; + + void Write(int residual_id, + int residual_offset, + double **jacobians, + SparseMatrix* base_jacobian); + + private: + Program* program_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_COMPRESSED_ROW_JACOBIAN_WRITER_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/compressed_row_sparse_matrix.cc b/extern/libmv/third_party/ceres/internal/ceres/compressed_row_sparse_matrix.cc new file mode 100644 index 00000000000..95edf5396af --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/compressed_row_sparse_matrix.cc @@ -0,0 +1,334 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/compressed_row_sparse_matrix.h" + +#include <algorithm> +#include <vector> +#include "ceres/matrix_proto.h" +#include "ceres/internal/port.h" + +namespace ceres { +namespace internal { +namespace { + +// Helper functor used by the constructor for reordering the contents +// of a TripletSparseMatrix. +struct RowColLessThan { + RowColLessThan(const int* rows, const int* cols) + : rows(rows), cols(cols) { + } + + bool operator()(const int x, const int y) const { + if (rows[x] == rows[y]) { + return (cols[x] < cols[y]); + } + return (rows[x] < rows[y]); + } + + const int* rows; + const int* cols; +}; + +} // namespace + +// This constructor gives you a semi-initialized CompressedRowSparseMatrix. +CompressedRowSparseMatrix::CompressedRowSparseMatrix(int num_rows, + int num_cols, + int max_num_nonzeros) { + num_rows_ = num_rows; + num_cols_ = num_cols; + max_num_nonzeros_ = max_num_nonzeros; + + VLOG(1) << "# of rows: " << num_rows_ << " # of columns: " << num_cols_ + << " max_num_nonzeros: " << max_num_nonzeros_ + << ". Allocating " << (num_rows_ + 1) * sizeof(int) + // NOLINT + max_num_nonzeros_ * sizeof(int) + // NOLINT + max_num_nonzeros_ * sizeof(double); // NOLINT + + rows_.reset(new int[num_rows_ + 1]); + cols_.reset(new int[max_num_nonzeros_]); + values_.reset(new double[max_num_nonzeros_]); + + fill(rows_.get(), rows_.get() + num_rows_ + 1, 0); + fill(cols_.get(), cols_.get() + max_num_nonzeros_, 0); + fill(values_.get(), values_.get() + max_num_nonzeros_, 0); +} + +CompressedRowSparseMatrix::CompressedRowSparseMatrix( + const TripletSparseMatrix& m) { + num_rows_ = m.num_rows(); + num_cols_ = m.num_cols(); + max_num_nonzeros_ = m.max_num_nonzeros(); + + // index is the list of indices into the TripletSparseMatrix m. + vector<int> index(m.num_nonzeros(), 0); + for (int i = 0; i < m.num_nonzeros(); ++i) { + index[i] = i; + } + + // Sort index such that the entries of m are ordered by row and ties + // are broken by column. + sort(index.begin(), index.end(), RowColLessThan(m.rows(), m.cols())); + + VLOG(1) << "# of rows: " << num_rows_ << " # of columns: " << num_cols_ + << " max_num_nonzeros: " << max_num_nonzeros_ + << ". Allocating " << (num_rows_ + 1) * sizeof(int) + // NOLINT + max_num_nonzeros_ * sizeof(int) + // NOLINT + max_num_nonzeros_ * sizeof(double); // NOLINT + + rows_.reset(new int[num_rows_ + 1]); + cols_.reset(new int[max_num_nonzeros_]); + values_.reset(new double[max_num_nonzeros_]); + + // rows_ = 0 + fill(rows_.get(), rows_.get() + num_rows_ + 1, 0); + + // Copy the contents of the cols and values array in the order given + // by index and count the number of entries in each row. + for (int i = 0; i < m.num_nonzeros(); ++i) { + const int idx = index[i]; + ++rows_[m.rows()[idx] + 1]; + cols_[i] = m.cols()[idx]; + values_[i] = m.values()[idx]; + } + + // Find the cumulative sum of the row counts. + for (int i = 1; i < num_rows_ + 1; ++i) { + rows_[i] += rows_[i-1]; + } + + CHECK_EQ(num_nonzeros(), m.num_nonzeros()); +} + +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +CompressedRowSparseMatrix::CompressedRowSparseMatrix( + const SparseMatrixProto& outer_proto) { + CHECK(outer_proto.has_compressed_row_matrix()); + + const CompressedRowSparseMatrixProto& proto = + outer_proto.compressed_row_matrix(); + + num_rows_ = proto.num_rows(); + num_cols_ = proto.num_cols(); + + rows_.reset(new int[proto.rows_size()]); + cols_.reset(new int[proto.cols_size()]); + values_.reset(new double[proto.values_size()]); + + for (int i = 0; i < proto.rows_size(); ++i) { + rows_[i] = proto.rows(i); + } + + CHECK_EQ(proto.rows_size(), num_rows_ + 1); + CHECK_EQ(proto.cols_size(), proto.values_size()); + CHECK_EQ(proto.cols_size(), rows_[num_rows_]); + + for (int i = 0; i < proto.cols_size(); ++i) { + cols_[i] = proto.cols(i); + values_[i] = proto.values(i); + } + + max_num_nonzeros_ = proto.cols_size(); +} +#endif + +CompressedRowSparseMatrix::CompressedRowSparseMatrix(const double* diagonal, + int num_rows) { + CHECK_NOTNULL(diagonal); + + num_rows_ = num_rows; + num_cols_ = num_rows; + max_num_nonzeros_ = num_rows; + + rows_.reset(new int[num_rows_ + 1]); + cols_.reset(new int[num_rows_]); + values_.reset(new double[num_rows_]); + + rows_[0] = 0; + for (int i = 0; i < num_rows_; ++i) { + cols_[i] = i; + values_[i] = diagonal[i]; + rows_[i + 1] = i + 1; + } + + CHECK_EQ(num_nonzeros(), num_rows); +} + +CompressedRowSparseMatrix::~CompressedRowSparseMatrix() { +} + +void CompressedRowSparseMatrix::SetZero() { + fill(values_.get(), values_.get() + num_nonzeros(), 0.0); +} + +void CompressedRowSparseMatrix::RightMultiply(const double* x, + double* y) const { + CHECK_NOTNULL(x); + CHECK_NOTNULL(y); + + for (int r = 0; r < num_rows_; ++r) { + for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) { + y[r] += values_[idx] * x[cols_[idx]]; + } + } +} + +void CompressedRowSparseMatrix::LeftMultiply(const double* x, double* y) const { + CHECK_NOTNULL(x); + CHECK_NOTNULL(y); + + for (int r = 0; r < num_rows_; ++r) { + for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) { + y[cols_[idx]] += values_[idx] * x[r]; + } + } +} + +void CompressedRowSparseMatrix::SquaredColumnNorm(double* x) const { + CHECK_NOTNULL(x); + + fill(x, x + num_cols_, 0.0); + for (int idx = 0; idx < rows_[num_rows_]; ++idx) { + x[cols_[idx]] += values_[idx] * values_[idx]; + } +} + +void CompressedRowSparseMatrix::ScaleColumns(const double* scale) { + CHECK_NOTNULL(scale); + + for (int idx = 0; idx < rows_[num_rows_]; ++idx) { + values_[idx] *= scale[cols_[idx]]; + } +} + +void CompressedRowSparseMatrix::ToDenseMatrix(Matrix* dense_matrix) const { + CHECK_NOTNULL(dense_matrix); + dense_matrix->resize(num_rows_, num_cols_); + dense_matrix->setZero(); + + for (int r = 0; r < num_rows_; ++r) { + for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) { + (*dense_matrix)(r, cols_[idx]) = values_[idx]; + } + } +} + +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +void CompressedRowSparseMatrix::ToProto(SparseMatrixProto* outer_proto) const { + CHECK_NOTNULL(outer_proto); + + outer_proto->Clear(); + CompressedRowSparseMatrixProto* proto + = outer_proto->mutable_compressed_row_matrix(); + + proto->set_num_rows(num_rows_); + proto->set_num_cols(num_cols_); + + for (int r = 0; r < num_rows_ + 1; ++r) { + proto->add_rows(rows_[r]); + } + + for (int idx = 0; idx < rows_[num_rows_]; ++idx) { + proto->add_cols(cols_[idx]); + proto->add_values(values_[idx]); + } +} +#endif + +void CompressedRowSparseMatrix::DeleteRows(int delta_rows) { + CHECK_GE(delta_rows, 0); + CHECK_LE(delta_rows, num_rows_); + + int new_num_rows = num_rows_ - delta_rows; + + num_rows_ = new_num_rows; + int* new_rows = new int[num_rows_ + 1]; + copy(rows_.get(), rows_.get() + num_rows_ + 1, new_rows); + rows_.reset(new_rows); +} + +void CompressedRowSparseMatrix::AppendRows(const CompressedRowSparseMatrix& m) { + CHECK_EQ(m.num_cols(), num_cols_); + + // Check if there is enough space. If not, then allocate new arrays + // to hold the combined matrix and copy the contents of this matrix + // into it. + if (max_num_nonzeros_ < num_nonzeros() + m.num_nonzeros()) { + int new_max_num_nonzeros = num_nonzeros() + m.num_nonzeros(); + + VLOG(1) << "Reallocating " << sizeof(int) * new_max_num_nonzeros; // NOLINT + + int* new_cols = new int[new_max_num_nonzeros]; + copy(cols_.get(), cols_.get() + max_num_nonzeros_, new_cols); + cols_.reset(new_cols); + + double* new_values = new double[new_max_num_nonzeros]; + copy(values_.get(), values_.get() + max_num_nonzeros_, new_values); + values_.reset(new_values); + + max_num_nonzeros_ = new_max_num_nonzeros; + } + + // Copy the contents of m into this matrix. + copy(m.cols(), m.cols() + m.num_nonzeros(), cols_.get() + num_nonzeros()); + copy(m.values(), + m.values() + m.num_nonzeros(), + values_.get() + num_nonzeros()); + + // Create the new rows array to hold the enlarged matrix. + int* new_rows = new int[num_rows_ + m.num_rows() + 1]; + // The first num_rows_ entries are the same + copy(rows_.get(), rows_.get() + num_rows_, new_rows); + + // new_rows = [rows_, m.row() + rows_[num_rows_]] + fill(new_rows + num_rows_, + new_rows + num_rows_ + m.num_rows() + 1, + rows_[num_rows_]); + + for (int r = 0; r < m.num_rows() + 1; ++r) { + new_rows[num_rows_ + r] += m.rows()[r]; + } + + rows_.reset(new_rows); + num_rows_ += m.num_rows(); +} + +void CompressedRowSparseMatrix::ToTextFile(FILE* file) const { + CHECK_NOTNULL(file); + for (int r = 0; r < num_rows_; ++r) { + for (int idx = rows_[r]; idx < rows_[r + 1]; ++idx) { + fprintf(file, "% 10d % 10d %17f\n", r, cols_[idx], values_[idx]); + } + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/compressed_row_sparse_matrix.h b/extern/libmv/third_party/ceres/internal/ceres/compressed_row_sparse_matrix.h new file mode 100644 index 00000000000..9a39d28e111 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/compressed_row_sparse_matrix.h @@ -0,0 +1,129 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_COMPRESSED_ROW_SPARSE_MATRIX_H_ +#define CERES_INTERNAL_COMPRESSED_ROW_SPARSE_MATRIX_H_ + +#include <glog/logging.h> +#include "ceres/sparse_matrix.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/macros.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +class SparseMatrixProto; + +class CompressedRowSparseMatrix : public SparseMatrix { + public: + // Build a matrix with the same content as the TripletSparseMatrix + // m. TripletSparseMatrix objects are easier to construct + // incrementally, so we use them to initialize SparseMatrix + // objects. + // + // We assume that m does not have any repeated entries. + explicit CompressedRowSparseMatrix(const TripletSparseMatrix& m); +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS + explicit CompressedRowSparseMatrix(const SparseMatrixProto& proto); +#endif + + // Use this constructor only if you know what you are doing. This + // creates a "blank" matrix with the appropriate amount of memory + // allocated. However, the object itself is in an inconsistent state + // as the rows and cols matrices do not match the values of + // num_rows, num_cols and max_num_nonzeros. + // + // The use case for this constructor is that when the user knows the + // size of the matrix to begin with and wants to update the layout + // manually, instead of going via the indirect route of first + // constructing a TripletSparseMatrix, which leads to more than + // double the peak memory usage. + CompressedRowSparseMatrix(int num_rows, + int num_cols, + int max_num_nonzeros); + + // Build a square sparse diagonal matrix with num_rows rows and + // columns. The diagonal m(i,i) = diagonal(i); + CompressedRowSparseMatrix(const double* diagonal, int num_rows); + + virtual ~CompressedRowSparseMatrix(); + + // SparseMatrix interface. + virtual void SetZero(); + virtual void RightMultiply(const double* x, double* y) const; + virtual void LeftMultiply(const double* x, double* y) const; + virtual void SquaredColumnNorm(double* x) const; + virtual void ScaleColumns(const double* scale); + + virtual void ToDenseMatrix(Matrix* dense_matrix) const; +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS + virtual void ToProto(SparseMatrixProto* proto) const; +#endif + virtual void ToTextFile(FILE* file) const; + virtual int num_rows() const { return num_rows_; } + virtual int num_cols() const { return num_cols_; } + virtual int num_nonzeros() const { return rows_[num_rows_]; } + virtual const double* values() const { return values_.get(); } + virtual double* mutable_values() { return values_.get(); } + + // Delete the bottom delta_rows. + // num_rows -= delta_rows + void DeleteRows(int delta_rows); + + // Append the contents of m to the bottom of this matrix. m must + // have the same number of columns as this matrix. + void AppendRows(const CompressedRowSparseMatrix& m); + + // Low level access methods that expose the structure of the matrix. + const int* cols() const { return cols_.get(); } + int* mutable_cols() { return cols_.get(); } + + const int* rows() const { return rows_.get(); } + int* mutable_rows() { return rows_.get(); } + + private: + scoped_array<int> cols_; + scoped_array<int> rows_; + scoped_array<double> values_; + + int num_rows_; + int num_cols_; + + int max_num_nonzeros_; + + DISALLOW_COPY_AND_ASSIGN(CompressedRowSparseMatrix); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_COMPRESSED_ROW_SPARSE_MATRIX_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/conditioned_cost_function.cc b/extern/libmv/third_party/ceres/internal/ceres/conditioned_cost_function.cc new file mode 100644 index 00000000000..ca80bfb9c9d --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/conditioned_cost_function.cc @@ -0,0 +1,130 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: wjr@google.com (William Rucklidge) +// +// This file contains the implementation of the conditioned cost function. + +#include "ceres/conditioned_cost_function.h" + +#include <cstddef> + +#include <glog/logging.h> +#include "ceres/stl_util.h" +#include "ceres/internal/eigen.h" +#include "ceres/types.h" + +namespace ceres { + +// This cost function has the same dimensions (parameters, residuals) as +// the one it's wrapping. +ConditionedCostFunction::ConditionedCostFunction( + CostFunction* wrapped_cost_function, + const vector<CostFunction*>& conditioners, + Ownership ownership) + : wrapped_cost_function_(wrapped_cost_function), + conditioners_(conditioners), + ownership_(ownership) { + // Set up our dimensions. + set_num_residuals(wrapped_cost_function_->num_residuals()); + *mutable_parameter_block_sizes() = + wrapped_cost_function_->parameter_block_sizes(); + + // Sanity-check the conditioners' dimensions. + CHECK_EQ(wrapped_cost_function_->num_residuals(), conditioners_.size()); + for (int i = 0; i < wrapped_cost_function_->num_residuals(); i++) { + if (conditioners[i]) { + CHECK_EQ(1, conditioners[i]->num_residuals()); + CHECK_EQ(1, conditioners[i]->parameter_block_sizes().size()); + CHECK_EQ(1, conditioners[i]->parameter_block_sizes()[0]); + } + } +} + +ConditionedCostFunction::~ConditionedCostFunction() { + if (ownership_ == TAKE_OWNERSHIP) { + STLDeleteElements(&conditioners_); + } else { + wrapped_cost_function_.release(); + } +} + +bool ConditionedCostFunction::Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const { + bool success = wrapped_cost_function_->Evaluate(parameters, residuals, + jacobians); + if (!success) { + return false; + } + + for (int r = 0; r < wrapped_cost_function_->num_residuals(); r++) { + // On output, we want to have + // residuals[r] = conditioners[r](wrapped_residuals[r]) + // For parameter block i, column c, + // jacobians[i][r*parameter_block_size_[i] + c] = + // = d residual[r] / d parameters[i][c] + // = conditioners[r]'(wrapped_residuals[r]) * + // d wrapped_residuals[r] / d parameters[i][c] + if (conditioners_[r]) { + double conditioner_derivative; + double* conditioner_derivative_pointer = &conditioner_derivative; + double** conditioner_derivative_pointer2 = + &conditioner_derivative_pointer; + if (!jacobians) { + conditioner_derivative_pointer2 = NULL; + } + + double unconditioned_residual = residuals[r]; + double* parameter_pointer = &unconditioned_residual; + success = conditioners_[r]->Evaluate(¶meter_pointer, + &residuals[r], + conditioner_derivative_pointer2); + if (!success) { + return false; + } + + if (jacobians) { + for (int i = 0; + i < wrapped_cost_function_->parameter_block_sizes().size(); + i++) { + if (jacobians[i]) { + int parameter_block_size = + wrapped_cost_function_->parameter_block_sizes()[i]; + VectorRef jacobian_row(jacobians[i] + r * parameter_block_size, + parameter_block_size, 1); + jacobian_row *= conditioner_derivative; + } + } + } + } + } + return true; +} + +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/conjugate_gradients_solver.cc b/extern/libmv/third_party/ceres/internal/ceres/conjugate_gradients_solver.cc new file mode 100644 index 00000000000..75f9e043fa5 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/conjugate_gradients_solver.cc @@ -0,0 +1,233 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// A preconditioned conjugate gradients solver +// (ConjugateGradientsSolver) for positive semidefinite linear +// systems. +// +// We have also augmented the termination criterion used by this +// solver to support not just residual based termination but also +// termination based on decrease in the value of the quadratic model +// that CG optimizes. + +#include "ceres/conjugate_gradients_solver.h" + +#include <cmath> +#include <cstddef> +#include <glog/logging.h> +#include "ceres/linear_operator.h" +#include "ceres/internal/eigen.h" +#include "ceres/types.h" +#include "ceres/jet.h" + +namespace ceres { +namespace internal { +namespace { + +bool IsZeroOrInfinity(double x) { + return ((x == 0.0) || (isinf(x))); +} + +// Constant used in the MATLAB implementation ~ 2 * eps. +const double kEpsilon = 2.2204e-16; + +} // namespace + +ConjugateGradientsSolver::ConjugateGradientsSolver( + const LinearSolver::Options& options) + : options_(options) { +} + +LinearSolver::Summary ConjugateGradientsSolver::Solve( + LinearOperator* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x) { + CHECK_NOTNULL(A); + CHECK_NOTNULL(x); + CHECK_NOTNULL(b); + CHECK_EQ(A->num_rows(), A->num_cols()); + + LinearSolver::Summary summary; + summary.termination_type = MAX_ITERATIONS; + summary.num_iterations = 0; + + int num_cols = A->num_cols(); + VectorRef xref(x, num_cols); + ConstVectorRef bref(b, num_cols); + + double norm_b = bref.norm(); + if (norm_b == 0.0) { + xref.setZero(); + summary.termination_type = TOLERANCE; + return summary; + } + + Vector r(num_cols); + Vector p(num_cols); + Vector z(num_cols); + Vector tmp(num_cols); + + double tol_r = per_solve_options.r_tolerance * norm_b; + + tmp.setZero(); + A->RightMultiply(x, tmp.data()); + r = bref - tmp; + double norm_r = r.norm(); + + if (norm_r <= tol_r) { + summary.termination_type = TOLERANCE; + return summary; + } + + double rho = 1.0; + + // Initial value of the quadratic model Q = x'Ax - 2 * b'x. + double Q0 = -1.0 * xref.dot(bref + r); + + for (summary.num_iterations = 1; + summary.num_iterations < options_.max_num_iterations; + ++summary.num_iterations) { + VLOG(2) << "cg iteration " << summary.num_iterations; + + // Apply preconditioner + if (per_solve_options.preconditioner != NULL) { + z.setZero(); + per_solve_options.preconditioner->RightMultiply(r.data(), z.data()); + } else { + z = r; + } + + double last_rho = rho; + rho = r.dot(z); + + if (IsZeroOrInfinity(rho)) { + LOG(ERROR) << "Numerical failure. rho = " << rho; + summary.termination_type = FAILURE; + break; + }; + + if (summary.num_iterations == 1) { + p = z; + } else { + double beta = rho / last_rho; + if (IsZeroOrInfinity(beta)) { + LOG(ERROR) << "Numerical failure. beta = " << beta; + summary.termination_type = FAILURE; + break; + } + p = z + beta * p; + } + + Vector& q = z; + q.setZero(); + A->RightMultiply(p.data(), q.data()); + double pq = p.dot(q); + + if ((pq <= 0) || isinf(pq)) { + LOG(ERROR) << "Numerical failure. pq = " << pq; + summary.termination_type = FAILURE; + break; + } + + double alpha = rho / pq; + if (isinf(alpha)) { + LOG(ERROR) << "Numerical failure. alpha " << alpha; + summary.termination_type = FAILURE; + break; + } + + xref = xref + alpha * p; + + // Ideally we would just use the update r = r - alpha*q to keep + // track of the residual vector. However this estimate tends to + // drift over time due to round off errors. Thus every + // residual_reset_period iterations, we calculate the residual as + // r = b - Ax. We do not do this every iteration because this + // requires an additional matrix vector multiply which would + // double the complexity of the CG algorithm. + if (summary.num_iterations % options_.residual_reset_period == 0) { + tmp.setZero(); + A->RightMultiply(x, tmp.data()); + r = bref - tmp; + } else { + r = r - alpha * q; + } + + // Quadratic model based termination. + // Q1 = x'Ax - 2 * b' x. + double Q1 = -1.0 * xref.dot(bref + r); + + // For PSD matrices A, let + // + // Q(x) = x'Ax - 2b'x + // + // be the cost of the quadratic function defined by A and b. Then, + // the solver terminates at iteration i if + // + // i * (Q(x_i) - Q(x_i-1)) / Q(x_i) < q_tolerance. + // + // This termination criterion is more useful when using CG to + // solve the Newton step. This particular convergence test comes + // from Stephen Nash's work on truncated Newton + // methods. References: + // + // 1. Stephen G. Nash & Ariela Sofer, Assessing A Search + // Direction Within A Truncated Newton Method, Operation + // Research Letters 9(1990) 219-221. + // + // 2. Stephen G. Nash, A Survey of Truncated Newton Methods, + // Journal of Computational and Applied Mathematics, + // 124(1-2), 45-59, 2000. + // + double zeta = summary.num_iterations * (Q1 - Q0) / Q1; + VLOG(2) << "Q termination: zeta " << zeta + << " " << per_solve_options.q_tolerance; + if (zeta < per_solve_options.q_tolerance) { + summary.termination_type = TOLERANCE; + break; + } + Q0 = Q1; + + // Residual based termination. + norm_r = r. norm(); + VLOG(2) << "R termination: norm_r " << norm_r + << " " << tol_r; + if (norm_r <= tol_r) { + summary.termination_type = TOLERANCE; + break; + } + } + + return summary; +}; + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/conjugate_gradients_solver.h b/extern/libmv/third_party/ceres/internal/ceres/conjugate_gradients_solver.h new file mode 100644 index 00000000000..57f99e31db7 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/conjugate_gradients_solver.h @@ -0,0 +1,74 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Preconditioned Conjugate Gradients based solver for positive +// semidefinite linear systems. + +#ifndef CERES_INTERNAL_CONJUGATE_GRADIENTS_SOLVER_H_ +#define CERES_INTERNAL_CONJUGATE_GRADIENTS_SOLVER_H_ + +#include "ceres/linear_solver.h" +#include "ceres/internal/macros.h" + +namespace ceres { +namespace internal { + +class LinearOperator; + +// This class implements the now classical Conjugate Gradients +// algorithm of Hestenes & Stiefel for solving postive semidefinite +// linear sytems. Optionally it can use a preconditioner also to +// reduce the condition number of the linear system and improve the +// convergence rate. Modern references for Conjugate Gradients are the +// books by Yousef Saad and Trefethen & Bau. This implementation of CG +// has been augmented with additional termination tests that are +// needed for forcing early termination when used as part of an +// inexact Newton solver. +// +// For more details see the documentation for +// LinearSolver::PerSolveOptions::r_tolerance and +// LinearSolver::PerSolveOptions::q_tolerance in linear_solver.h. +class ConjugateGradientsSolver : public LinearSolver { + public: + explicit ConjugateGradientsSolver(const LinearSolver::Options& options); + virtual Summary Solve(LinearOperator* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x); + + private: + const LinearSolver::Options options_; + DISALLOW_COPY_AND_ASSIGN(ConjugateGradientsSolver); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_CONJUGATE_GRADIENTS_SOLVER_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/corrector.cc b/extern/libmv/third_party/ceres/internal/ceres/corrector.cc new file mode 100644 index 00000000000..4ca2c6f6c86 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/corrector.cc @@ -0,0 +1,125 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/corrector.h" + +#include <cstddef> +#include <cmath> +#include <glog/logging.h> +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +Corrector::Corrector(double sq_norm, const double rho[3]) { + CHECK_GE(sq_norm, 0.0); + CHECK_GT(rho[1], 0.0); + sqrt_rho1_ = sqrt(rho[1]); + + // If sq_norm = 0.0, the correction becomes trivial, the residual + // and the jacobian are scaled by the squareroot of the derivative + // of rho. Handling this case explicitly avoids the divide by zero + // error that would occur below. + // + // The case where rho'' < 0 also gets special handling. Technically + // it shouldn't, and the computation of the scaling should proceed + // as below, however we found in experiments that applying the + // curvature correction when rho'' < 0, which is the case when we + // are in the outlier region slows down the convergence of the + // algorithm significantly. + // + // Thus, we have divided the action of the robustifier into two + // parts. In the inliner region, we do the full second order + // correction which re-wights the gradient of the function by the + // square root of the derivative of rho, and the Gauss-Newton + // Hessian gets both the scaling and the rank-1 curvature + // correction. Normaly, alpha is upper bounded by one, but with this + // change, alpha is bounded above by zero. + // + // Empirically we have observed that the full Triggs correction and + // the clamped correction both start out as very good approximations + // to the loss function when we are in the convex part of the + // function, but as the function starts transitioning from convex to + // concave, the Triggs approximation diverges more and more and + // ultimately becomes linear. The clamped Triggs model however + // remains quadratic. + // + // The reason why the Triggs approximation becomes so poor is + // because the curvature correction that it applies to the gauss + // newton hessian goes from being a full rank correction to a rank + // deficient correction making the inversion of the Hessian fraught + // with all sorts of misery and suffering. + // + // The clamped correction retains its quadratic nature and inverting it + // is always well formed. + if ((sq_norm == 0.0) || (rho[2] <= 0.0)) { + residual_scaling_ = sqrt_rho1_; + alpha_sq_norm_ = 0.0; + return; + } + + // Calculate the smaller of the two solutions to the equation + // + // 0.5 * alpha^2 - alpha - rho'' / rho' * z'z = 0. + // + // Start by calculating the discriminant D. + const double D = 1.0 + 2.0 * sq_norm*rho[2] / rho[1]; + + // Since both rho[1] and rho[2] are guaranteed to be positive at + // this point, we know that D > 1.0. + + const double alpha = 1.0 - sqrt(D); + + // Calculate the constants needed by the correction routines. + residual_scaling_ = sqrt_rho1_ / (1 - alpha); + alpha_sq_norm_ = alpha / sq_norm; +} + +void Corrector::CorrectResiduals(int nrow, double* residuals) { + DCHECK(residuals != NULL); + VectorRef r_ref(residuals, nrow); + // Equation 11 in BANS. + r_ref *= residual_scaling_; +} + +void Corrector::CorrectJacobian(int nrow, int ncol, + double* residuals, double* jacobian) { + DCHECK(residuals != NULL); + DCHECK(jacobian != NULL); + ConstVectorRef r_ref(residuals, nrow); + MatrixRef j_ref(jacobian, nrow, ncol); + + // Equation 11 in BANS. + j_ref = sqrt_rho1_ * (j_ref - alpha_sq_norm_ * + r_ref * (r_ref.transpose() * j_ref)); +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/corrector.h b/extern/libmv/third_party/ceres/internal/ceres/corrector.h new file mode 100644 index 00000000000..9914641cb01 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/corrector.h @@ -0,0 +1,88 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Class definition for the object that is responsible for applying a +// second order correction to the Gauss-Newton based on the ideas in +// BANS by Triggs et al. + +#ifndef CERES_INTERNAL_CORRECTOR_H_ +#define CERES_INTERNAL_CORRECTOR_H_ + +namespace ceres { +namespace internal { + +// Corrector is responsible for applying the second order correction +// to the residual and jacobian of a least squares problem based on a +// radial robust loss. +// +// The key idea here is to look at the expressions for the robustified +// gauss newton approximation and then take its squareroot to get the +// corresponding corrections to the residual and jacobian. For the +// full expressions see Eq. 10 and 11 in BANS by Triggs et al. +class Corrector { + public: + // The constructor takes the squared norm, the value, the first and + // second derivatives of the LossFunction. It precalculates some of + // the constants that are needed to apply the correction. The + // correction constant alpha is constrained to be smaller than 1, if + // it becomes larger than 1, then it will reverse the sign of the + // residual and the correction. If alpha is equal to 1 will result + // in a divide by zero error. Thus we constrain alpha to be upper + // bounded by 1 - epsilon_. + // + // rho[1] needs to be positive. The constructor will crash if this + // condition is not met. + // + // In practical use CorrectJacobian should always be called before + // CorrectResidual, because the jacobian correction depends on the + // value of the uncorrected residual values. + explicit Corrector(double sq_norm, const double rho[3]); + + // residuals *= sqrt(rho[1]) / (1 - alpha) + void CorrectResiduals(int nrow, double* residuals); + + // jacobian = sqrt(rho[1]) * jacobian - + // sqrt(rho[1]) * alpha / sq_norm * residuals residuals' * jacobian. + // + // The method assumes that the jacobian has row-major storage. It is + // the caller's responsibility to ensure that the pointer to + // jacobian is not null. + void CorrectJacobian(int nrow, int ncol, + double* residuals, double* jacobian); + + private: + double sqrt_rho1_; + double residual_scaling_; + double alpha_sq_norm_; +}; +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_CORRECTOR_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/dense_jacobian_writer.h b/extern/libmv/third_party/ceres/internal/ceres/dense_jacobian_writer.h new file mode 100644 index 00000000000..1177b83a556 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/dense_jacobian_writer.h @@ -0,0 +1,110 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// A jacobian writer that writes to dense Eigen matrices. + +#ifndef CERES_INTERNAL_DENSE_JACOBIAN_WRITER_H_ +#define CERES_INTERNAL_DENSE_JACOBIAN_WRITER_H_ + +#include "ceres/casts.h" +#include "ceres/dense_sparse_matrix.h" +#include "ceres/parameter_block.h" +#include "ceres/program.h" +#include "ceres/residual_block.h" +#include "ceres/scratch_evaluate_preparer.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +class DenseJacobianWriter { + public: + DenseJacobianWriter(Evaluator::Options /* ignored */, + Program* program) + : program_(program) { + } + + // JacobianWriter interface. + + // Since the dense matrix has different layout than that assumed by the cost + // functions, use scratch space to store the jacobians temporarily then copy + // them over to the larger jacobian later. + ScratchEvaluatePreparer* CreateEvaluatePreparers(int num_threads) { + return ScratchEvaluatePreparer::Create(*program_, num_threads); + } + + SparseMatrix* CreateJacobian() const { + return new DenseSparseMatrix(program_->NumResiduals(), + program_->NumEffectiveParameters()); + } + + void Write(int residual_id, + int residual_offset, + double **jacobians, + SparseMatrix* jacobian) { + DenseSparseMatrix* dense_jacobian; + if (jacobian != NULL) { + dense_jacobian = down_cast<DenseSparseMatrix*>(jacobian); + } + const ResidualBlock* residual_block = + program_->residual_blocks()[residual_id]; + int num_parameter_blocks = residual_block->NumParameterBlocks(); + int num_residuals = residual_block->NumResiduals(); + + // Now copy the jacobians for each parameter into the dense jacobian matrix. + for (int j = 0; j < num_parameter_blocks; ++j) { + ParameterBlock* parameter_block = residual_block->parameter_blocks()[j]; + + // If the parameter block is fixed, then there is nothing to do. + if (parameter_block->IsConstant()) { + continue; + } + + int parameter_block_size = parameter_block->LocalSize(); + MatrixRef parameter_jacobian(jacobians[j], + num_residuals, + parameter_block_size); + + dense_jacobian->mutable_matrix().block( + residual_offset, + parameter_block->delta_offset(), + num_residuals, + parameter_block_size) = parameter_jacobian; + } + } + + private: + Program* program_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_DENSE_JACOBIAN_WRITER_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/dense_qr_solver.cc b/extern/libmv/third_party/ceres/internal/ceres/dense_qr_solver.cc new file mode 100644 index 00000000000..328505404d7 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/dense_qr_solver.cc @@ -0,0 +1,93 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/dense_qr_solver.h" + +#include <cstddef> + +#include "Eigen/Dense" +#include "ceres/linear_solver.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +DenseQRSolver::DenseQRSolver(const LinearSolver::Options& options) + : options_(options) {} + +LinearSolver::Summary DenseQRSolver::SolveImpl( + DenseSparseMatrix* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x) { + const int num_rows = A->num_rows(); + const int num_cols = A->num_cols(); + VLOG(2) << "DenseQRSolver: " + << num_rows << " x " << num_cols << " system."; + + if (per_solve_options.D != NULL) { + // Temporarily append a diagonal block to the A matrix, but undo + // it before returning the matrix to the user. + A->AppendDiagonal(per_solve_options.D); + } + + // rhs = [b;0] to account for the additional rows in the lhs. + Vector rhs(num_rows + ((per_solve_options.D !=NULL) ? num_cols : 0)); + rhs.setZero(); + rhs.head(num_rows) = ConstVectorRef(b, num_rows); + + // Solve the system. + VectorRef(x, num_cols) = A->matrix().colPivHouseholderQr().solve(rhs); + + VLOG(3) << "A:\n" << A->matrix(); + VLOG(3) << "x:\n" << VectorRef(x, num_cols); + VLOG(3) << "b:\n" << rhs; + VLOG(3) << "error: " << (A->matrix() * VectorRef(x, num_cols) - rhs).norm(); + + + if (per_solve_options.D != NULL) { + // Undo the modifications to the matrix A. + A->RemoveDiagonal(); + } + + // We always succeed, since the QR solver returns the best solution + // it can. It is the job of the caller to determine if the solution + // is good enough or not. + LinearSolver::Summary summary; + summary.num_iterations = 1; + summary.termination_type = TOLERANCE; + return summary; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/dense_qr_solver.h b/extern/libmv/third_party/ceres/internal/ceres/dense_qr_solver.h new file mode 100644 index 00000000000..990c8d445eb --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/dense_qr_solver.h @@ -0,0 +1,99 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Solve dense rectangular systems Ax = b using the QR factoriztion. +#ifndef CERES_INTERNAL_DENSE_QR_SOLVER_H_ +#define CERES_INTERNAL_DENSE_QR_SOLVER_H_ + +#include "ceres/linear_solver.h" +#include "ceres/internal/macros.h" + +namespace ceres { +namespace internal { + +class DenseSparseMatrix; + +// This class implements the LinearSolver interface for solving +// rectangular/unsymmetric (well constrained) linear systems of the +// form +// +// Ax = b +// +// Since there does not usually exist a solution that satisfies these +// equations, the solver instead solves the linear least squares +// problem +// +// min_x |Ax - b|^2 +// +// The solution strategy is based on computing the QR decomposition of +// A, i.e. +// +// A = QR +// +// Where Q is an orthonormal matrix and R is an upper triangular +// matrix. Then +// +// Ax = b +// QRx = b +// Q'QRx = Q'b +// Rx = Q'b +// x = R^{-1} Q'b +// +// If the PerSolveOptions struct has a non-null array D, then the +// augmented/regularized linear system +// +// [ A ]x = [b] +// [ diag(D) ] [0] +// +// is solved. +// +// This class uses the dense QR factorization routines from the Eigen +// library. This solver always returns a solution, it is the user's +// responsibility to judge if the solution is good enough for their +// purposes. +class DenseQRSolver: public DenseSparseMatrixSolver { + public: + explicit DenseQRSolver(const LinearSolver::Options& options); + + private: + virtual LinearSolver::Summary SolveImpl( + DenseSparseMatrix* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x); + + const LinearSolver::Options options_; + DISALLOW_COPY_AND_ASSIGN(DenseQRSolver); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_DENSE_QR_SOLVER_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/dense_sparse_matrix.cc b/extern/libmv/third_party/ceres/internal/ceres/dense_sparse_matrix.cc new file mode 100644 index 00000000000..5d392ba6c3b --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/dense_sparse_matrix.cc @@ -0,0 +1,197 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#include "ceres/dense_sparse_matrix.h" + +#include <algorithm> +#include "ceres/matrix_proto.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/port.h" + +namespace ceres { +namespace internal { + +DenseSparseMatrix::DenseSparseMatrix(int num_rows, int num_cols) + : has_diagonal_appended_(false), + has_diagonal_reserved_(false) { + // Allocate enough space for the diagonal. + m_.resize(num_rows, num_cols); + m_.setZero(); +} + +DenseSparseMatrix::DenseSparseMatrix(const TripletSparseMatrix& m) + : m_(Eigen::MatrixXd::Zero(m.num_rows(), m.num_cols())), + has_diagonal_appended_(false), + has_diagonal_reserved_(false) { + const double *values = m.values(); + const int *rows = m.rows(); + const int *cols = m.cols(); + int num_nonzeros = m.num_nonzeros(); + + for (int i = 0; i < num_nonzeros; ++i) { + m_(rows[i], cols[i]) += values[i]; + } +} + +DenseSparseMatrix::DenseSparseMatrix(const Matrix& m) + : m_(m), + has_diagonal_appended_(false), + has_diagonal_reserved_(false) { +} + +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +DenseSparseMatrix::DenseSparseMatrix(const SparseMatrixProto& outer_proto) + : m_(Eigen::MatrixXd::Zero( + outer_proto.dense_matrix().num_rows(), + outer_proto.dense_matrix().num_cols())), + has_diagonal_appended_(false), + has_diagonal_reserved_(false) { + const DenseSparseMatrixProto& proto = outer_proto.dense_matrix(); + for (int i = 0; i < m_.rows(); ++i) { + for (int j = 0; j < m_.cols(); ++j) { + m_(i, j) = proto.values(m_.cols() * i + j); + } + } +} +#endif + +void DenseSparseMatrix::SetZero() { + m_.setZero(); +} + +void DenseSparseMatrix::RightMultiply(const double* x, double* y) const { + VectorRef(y, num_rows()) += matrix() * ConstVectorRef(x, num_cols()); +} + +void DenseSparseMatrix::LeftMultiply(const double* x, double* y) const { + VectorRef(y, num_cols()) += + matrix().transpose() * ConstVectorRef(x, num_rows()); +} + +void DenseSparseMatrix::SquaredColumnNorm(double* x) const { + VectorRef(x, num_cols()) = m_.colwise().squaredNorm(); +} + +void DenseSparseMatrix::ScaleColumns(const double* scale) { + m_ *= ConstVectorRef(scale, num_cols()).asDiagonal(); +} + +void DenseSparseMatrix::ToDenseMatrix(Matrix* dense_matrix) const { + *dense_matrix = m_; +} + +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +void DenseSparseMatrix::ToProto(SparseMatrixProto* outer_proto) const { + CHECK(!has_diagonal_appended_) << "Not supported."; + outer_proto->Clear(); + DenseSparseMatrixProto* proto = outer_proto->mutable_dense_matrix(); + + proto->set_num_rows(num_rows()); + proto->set_num_cols(num_cols()); + + int num_nnz = num_nonzeros(); + for (int i = 0; i < num_nnz; ++i) { + proto->add_values(m_.data()[i]); + } +} +#endif + +void DenseSparseMatrix::AppendDiagonal(double *d) { + CHECK(!has_diagonal_appended_); + if (!has_diagonal_reserved_) { + Matrix tmp = m_; + m_.resize(m_.rows() + m_.cols(), m_.cols()); + m_.setZero(); + m_.block(0, 0, tmp.rows(), tmp.cols()) = tmp; + has_diagonal_reserved_ = true; + } + + m_.bottomLeftCorner(m_.cols(), m_.cols()) = + ConstVectorRef(d, m_.cols()).asDiagonal(); + has_diagonal_appended_ = true; +} + +void DenseSparseMatrix::RemoveDiagonal() { + CHECK(has_diagonal_appended_); + has_diagonal_appended_ = false; + // Leave the diagonal reserved. +} + +int DenseSparseMatrix::num_rows() const { + if (has_diagonal_reserved_ && !has_diagonal_appended_) { + return m_.rows() - m_.cols(); + } + return m_.rows(); +} + +int DenseSparseMatrix::num_cols() const { + return m_.cols(); +} + +int DenseSparseMatrix::num_nonzeros() const { + if (has_diagonal_reserved_ && !has_diagonal_appended_) { + return (m_.rows() - m_.cols()) * m_.cols(); + } + return m_.rows() * m_.cols(); +} + +ConstAlignedMatrixRef DenseSparseMatrix::matrix() const { + if (has_diagonal_reserved_ && !has_diagonal_appended_) { + return ConstAlignedMatrixRef( + m_.data(), m_.rows() - m_.cols(), m_.cols()); + } + return ConstAlignedMatrixRef(m_.data(), m_.rows(), m_.cols()); +} + +AlignedMatrixRef DenseSparseMatrix::mutable_matrix() { + if (has_diagonal_reserved_ && !has_diagonal_appended_) { + return AlignedMatrixRef( + m_.data(), m_.rows() - m_.cols(), m_.cols()); + } + return AlignedMatrixRef(m_.data(), m_.rows(), m_.cols()); +} + +void DenseSparseMatrix::ToTextFile(FILE* file) const { + CHECK_NOTNULL(file); + const int active_rows = + (has_diagonal_reserved_ && !has_diagonal_appended_) + ? (m_.rows() - m_.cols()) + : m_.rows(); + + for (int r = 0; r < active_rows; ++r) { + for (int c = 0; c < m_.cols(); ++c) { + fprintf(file, "% 10d % 10d %17f\n", r, c, m_(r, c)); + } + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/dense_sparse_matrix.h b/extern/libmv/third_party/ceres/internal/ceres/dense_sparse_matrix.h new file mode 100644 index 00000000000..416c2143c2c --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/dense_sparse_matrix.h @@ -0,0 +1,116 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// A dense matrix implemented under the SparseMatrix interface. + +#ifndef CERES_INTERNAL_DENSE_SPARSE_MATRIX_H_ +#define CERES_INTERNAL_DENSE_SPARSE_MATRIX_H_ + +#include <glog/logging.h> +#include "ceres/sparse_matrix.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/macros.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +class SparseMatrixProto; +class TripletSparseMatrix; + +class DenseSparseMatrix : public SparseMatrix { + public: + // Build a matrix with the same content as the TripletSparseMatrix + // m. This assumes that m does not have any repeated entries. + explicit DenseSparseMatrix(const TripletSparseMatrix& m); + explicit DenseSparseMatrix(const Matrix& m); +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS + explicit DenseSparseMatrix(const SparseMatrixProto& proto); +#endif + + DenseSparseMatrix(int num_rows, int num_cols); + + virtual ~DenseSparseMatrix() {} + + // SparseMatrix interface. + virtual void SetZero(); + virtual void RightMultiply(const double* x, double* y) const; + virtual void LeftMultiply(const double* x, double* y) const; + virtual void SquaredColumnNorm(double* x) const; + virtual void ScaleColumns(const double* scale); + virtual void ToDenseMatrix(Matrix* dense_matrix) const; +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS + virtual void ToProto(SparseMatrixProto* proto) const; +#endif + virtual void ToTextFile(FILE* file) const; + virtual int num_rows() const; + virtual int num_cols() const; + virtual int num_nonzeros() const; + virtual const double* values() const { return m_.data(); } + virtual double* mutable_values() { return m_.data(); } + + ConstAlignedMatrixRef matrix() const; + AlignedMatrixRef mutable_matrix(); + + // Only one diagonal can be appended at a time. The diagonal is appended to + // as a new set of rows, e.g. + // + // Original matrix: + // + // x x x + // x x x + // x x x + // + // After append diagonal (1, 2, 3): + // + // x x x + // x x x + // x x x + // 1 0 0 + // 0 2 0 + // 0 0 3 + // + // Calling RemoveDiagonal removes the block. It is a fatal error to append a + // diagonal to a matrix that already has an appended diagonal, and it is also + // a fatal error to remove a diagonal from a matrix that has none. + void AppendDiagonal(double *d); + void RemoveDiagonal(); + + private: + Matrix m_; + bool has_diagonal_appended_; + bool has_diagonal_reserved_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_DENSE_SPARSE_MATRIX_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/detect_structure.cc b/extern/libmv/third_party/ceres/internal/ceres/detect_structure.cc new file mode 100644 index 00000000000..e9755043bab --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/detect_structure.cc @@ -0,0 +1,114 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include <glog/logging.h> +#include "ceres/detect_structure.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +void DetectStructure(const CompressedRowBlockStructure& bs, + const int num_eliminate_blocks, + int* row_block_size, + int* e_block_size, + int* f_block_size) { + const int num_row_blocks = bs.rows.size(); + *row_block_size = 0; + *e_block_size = 0; + *f_block_size = 0; + + // Iterate over row blocks of the matrix, checking if row_block, + // e_block or f_block sizes remain constant. + for (int r = 0; r < num_row_blocks; ++r) { + const CompressedRow& row = bs.rows[r]; + // We do not care about the sizes of the blocks in rows which do + // not contain e_blocks. + if (row.cells.front().block_id >= num_eliminate_blocks) { + break; + } + const int e_block_id = row.cells.front().block_id; + + if (*row_block_size == 0) { + *row_block_size = row.block.size; + } else if (*row_block_size != Eigen::Dynamic && + *row_block_size != row.block.size) { + *row_block_size = Eigen::Dynamic; + VLOG(2) << "Dynamic row block size because the block size changed from " + << *row_block_size << " to " + << row.block.size; + } + + + if (*e_block_size == 0) { + *e_block_size = bs.cols[e_block_id].size; + } else if (*e_block_size != Eigen::Dynamic && + *e_block_size != bs.cols[e_block_id].size) { + *e_block_size = Eigen::Dynamic; + VLOG(2) << "Dynamic e block size because the block size changed from " + << *e_block_size << " to " + << bs.cols[e_block_id].size; + } + + if (*f_block_size == 0) { + if (row.cells.size() > 1) { + const int f_block_id = row.cells[1].block_id; + *f_block_size = bs.cols[f_block_id].size; + } + } else if (*f_block_size != Eigen::Dynamic) { + for (int c = 1; c < row.cells.size(); ++c) { + if (*f_block_size != bs.cols[row.cells[c].block_id].size) { + *f_block_size = Eigen::Dynamic; + VLOG(2) << "Dynamic f block size because the block size " + << "changed from " << *f_block_size << " to " + << bs.cols[row.cells[c].block_id].size; + break; + } + } + } + + const bool is_everything_dynamic = (*row_block_size == Eigen::Dynamic && + *e_block_size == Eigen::Dynamic && + *f_block_size == Eigen::Dynamic); + if (is_everything_dynamic) { + break; + } + } + + CHECK_NE(*row_block_size, 0) << "No rows found"; + CHECK_NE(*e_block_size, 0) << "No e type blocks found"; + VLOG(1) << "Schur complement static structure <" + << *row_block_size << "," + << *e_block_size << "," + << *f_block_size << ">."; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/detect_structure.h b/extern/libmv/third_party/ceres/internal/ceres/detect_structure.h new file mode 100644 index 00000000000..8af4f236690 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/detect_structure.h @@ -0,0 +1,63 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_DETECT_STRUCTURE_H_ +#define CERES_INTERNAL_DETECT_STRUCTURE_H_ + +#include "ceres/block_structure.h" + +namespace ceres { +namespace internal { + +// Detect static blocks in the problem sparsity. For rows containing +// e_blocks, we are interested in detecting if the size of the row +// blocks, e_blocks and the f_blocks remain constant. If they do, then +// we can use template specialization to improve the performance of +// the block level linear algebra operations used by the +// SchurEliminator. +// +// If a block size is not constant, we return Eigen::Dynamic as the +// value. This just means that the eliminator uses dynamically sized +// linear algebra operations rather than static operations whose size +// is known as compile time. +// +// For more details about e_blocks and f_blocks, see +// schur_complement.h. This information is used to initialized an +// appropriate template specialization of SchurEliminator. +void DetectStructure(const CompressedRowBlockStructure& bs, + const int num_eliminate_blocks, + int* row_block_size, + int* e_block_size, + int* f_block_size); + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_DETECT_STRUCTURE_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/evaluator.cc b/extern/libmv/third_party/ceres/internal/ceres/evaluator.cc new file mode 100644 index 00000000000..ea05aefec8c --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/evaluator.cc @@ -0,0 +1,71 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#include <glog/logging.h> +#include "ceres/evaluator.h" +#include "ceres/block_evaluate_preparer.h" +#include "ceres/block_jacobian_writer.h" +#include "ceres/compressed_row_jacobian_writer.h" +#include "ceres/scratch_evaluate_preparer.h" +#include "ceres/dense_jacobian_writer.h" +#include "ceres/program_evaluator.h" + +namespace ceres { +namespace internal { + +Evaluator::~Evaluator() {} + +Evaluator* Evaluator::Create(const Evaluator::Options& options, + Program* program, + string* error) { + switch (options.linear_solver_type) { + case DENSE_QR: + return new ProgramEvaluator<ScratchEvaluatePreparer, + DenseJacobianWriter>(options, + program); + case DENSE_SCHUR: + case SPARSE_SCHUR: + case ITERATIVE_SCHUR: + case CGNR: + return new ProgramEvaluator<BlockEvaluatePreparer, + BlockJacobianWriter>(options, + program); + case SPARSE_NORMAL_CHOLESKY: + return new ProgramEvaluator<ScratchEvaluatePreparer, + CompressedRowJacobianWriter>(options, + program); + default: + *error = "Invalid Linear Solver Type. Unable to create evaluator."; + return NULL; + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/evaluator.h b/extern/libmv/third_party/ceres/internal/ceres/evaluator.h new file mode 100644 index 00000000000..adefdd26660 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/evaluator.h @@ -0,0 +1,129 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// keir@google.com (Keir Mierle) + +#ifndef CERES_INTERNAL_EVALUATOR_H_ +#define CERES_INTERNAL_EVALUATOR_H_ + +#include <string> +#include "ceres/internal/port.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +class Program; +class SparseMatrix; + +// The Evaluator interface offers a way to interact with a least squares cost +// function that is useful for an optimizer that wants to minimize the least +// squares objective. This insulates the optimizer from issues like Jacobian +// storage, parameterization, etc. +class Evaluator { + public: + virtual ~Evaluator(); + + struct Options { + Options() + : num_threads(1), + num_eliminate_blocks(-1), + linear_solver_type(DENSE_QR) {} + + int num_threads; + int num_eliminate_blocks; + LinearSolverType linear_solver_type; + }; + + static Evaluator* Create(const Options& options, + Program* program, + string* error); + + // Build and return a sparse matrix for storing and working with the Jacobian + // of the objective function. The jacobian has dimensions + // NumEffectiveParameters() by NumParameters(), and is typically extremely + // sparse. Since the sparsity pattern of the Jacobian remains constant over + // the lifetime of the optimization problem, this method is used to + // instantiate a SparseMatrix object with the appropriate sparsity structure + // (which can be an expensive operation) and then reused by the optimization + // algorithm and the various linear solvers. + // + // It is expected that the classes implementing this interface will be aware + // of their client's requirements for the kind of sparse matrix storage and + // layout that is needed for an efficient implementation. For example + // CompressedRowOptimizationProblem creates a compressed row representation of + // the jacobian for use with CHOLMOD, where as BlockOptimizationProblem + // creates a BlockSparseMatrix representation of the jacobian for use in the + // Schur complement based methods. + virtual SparseMatrix* CreateJacobian() const = 0; + + // Evaluate the cost function for the given state. Returns the cost, + // residuals, and jacobian in the corresponding arguments. Both residuals and + // jacobian are optional; to avoid computing them, pass NULL. + // + // If non-NULL, the Jacobian must have a suitable sparsity pattern; only the + // values array of the jacobian is modified. + // + // state is an array of size NumParameters(), cost is a pointer to a single + // double, and residuals is an array of doubles of size NumResiduals(). + virtual bool Evaluate(const double* state, + double* cost, + double* residuals, + SparseMatrix* jacobian) = 0; + + // Make a change delta (of size NumEffectiveParameters()) to state (of size + // NumParameters()) and store the result in state_plus_delta. + // + // In the case that there are no parameterizations used, this is equivalent to + // + // state_plus_delta[i] = state[i] + delta[i] ; + // + // however, the mapping is more complicated in the case of parameterizations + // like quaternions. This is the same as the "Plus()" operation in + // local_parameterization.h, but operating over the entire state vector for a + // problem. + virtual bool Plus(const double* state, + const double* delta, + double* state_plus_delta) const = 0; + + // The number of parameters in the optimization problem. + virtual int NumParameters() const = 0; + + // This is the effective number of parameters that the optimizer may adjust. + // This applies when there are parameterizations on some of the parameters. + virtual int NumEffectiveParameters() const = 0; + + // The number of residuals in the optimization problem. + virtual int NumResiduals() const = 0; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_EVALUATOR_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/file.cc b/extern/libmv/third_party/ceres/internal/ceres/file.cc new file mode 100644 index 00000000000..5fc9d220861 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/file.cc @@ -0,0 +1,93 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// Really simple file IO. + +#include <cstdio> +#include <glog/logging.h> + +namespace ceres { +namespace internal { + +using std::string; + +void WriteStringToFileOrDie(const string &data, const string &filename) { + FILE* file_descriptor = fopen(filename.c_str(), "wb"); + if (!file_descriptor) { + LOG(FATAL) << "Couldn't write to file: " << filename; + } + fwrite(data.c_str(), 1, data.size(), file_descriptor); + fclose(file_descriptor); +} + +void ReadFileToStringOrDie(const string &filename, string *data) { + FILE* file_descriptor = file_descriptor = fopen(filename.c_str(), "r"); + + if (!file_descriptor) { + LOG(FATAL) << "Couldn't read file: " << filename; + } + + // Resize the input buffer appropriately. + fseek(file_descriptor, 0L, SEEK_END); + int num_bytes = ftell(file_descriptor); + data->resize(num_bytes); + + // Read the data. + fseek(file_descriptor, 0L, SEEK_SET); + int num_read = fread(&((*data)[0]), + sizeof((*data)[0]), + num_bytes, + file_descriptor); + if (num_read != num_bytes) { + LOG(FATAL) << "Couldn't read all of " << filename + << "expected bytes: " << num_bytes * sizeof((*data)[0]) + << "actual bytes: " << num_read; + } + fclose(file_descriptor); +} + +string JoinPath(const string& dirname, const string& basename) { +#ifdef _WIN32 + static const char separator = '\\'; +#else + static const char separator = '/'; +#endif // _WIN32 + + if ((!basename.empty() && basename[0] == separator) || dirname.empty()) { + return basename; + } else if (dirname[dirname.size() - 1] == separator) { + return dirname + basename; + } else { + return dirname + string(&separator, 1) + basename; + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/file.h b/extern/libmv/third_party/ceres/internal/ceres/file.h new file mode 100644 index 00000000000..4741d650646 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/file.h @@ -0,0 +1,52 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// Simple file IO support. This is a portability shim. + +#ifndef CERES_INTERNAL_FILE_H_ +#define CERES_INTERNAL_FILE_H_ + +#include <string> +#include "ceres/internal/port.h" + +namespace ceres { +namespace internal { + +void WriteStringToFileOrDie(const string &data, const string &filename); +void ReadFileToStringOrDie(const string &filename, string *data); + +// Join two path components, adding a slash if necessary. If basename is an +// absolute path then JoinPath ignores dirname and simply returns basename. +string JoinPath(const string& dirname, const string& basename); + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_FILE_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_2_2.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_2_2.cc new file mode 100644 index 00000000000..5529386e485 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_2_2.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 2, 2>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_2_3.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_2_3.cc new file mode 100644 index 00000000000..fd7af95192e --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_2_3.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 2, 3>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_2_4.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_2_4.cc new file mode 100644 index 00000000000..109483e9fc0 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_2_4.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 2, 4>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_2_d.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_2_d.cc new file mode 100644 index 00000000000..b93e82fe2fa --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_2_d.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 2, Dynamic>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_3_3.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_3_3.cc new file mode 100644 index 00000000000..86352c07304 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_3_3.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 3, 3>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_3_4.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_3_4.cc new file mode 100644 index 00000000000..200df7f5931 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_3_4.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 3, 4>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_3_9.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_3_9.cc new file mode 100644 index 00000000000..1fda3434bef --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_3_9.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 3, 9>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_3_d.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_3_d.cc new file mode 100644 index 00000000000..385cd2d70c9 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_3_d.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 3, Dynamic>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_4_3.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_4_3.cc new file mode 100644 index 00000000000..7b15d6366ac --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_4_3.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 4, 3>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_4_4.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_4_4.cc new file mode 100644 index 00000000000..29a610d743e --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_4_4.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 4, 4>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_4_d.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_4_d.cc new file mode 100644 index 00000000000..a3bc4dc6f83 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_2_4_d.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<2, 4, Dynamic>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_4_4_2.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_4_4_2.cc new file mode 100644 index 00000000000..f71a4f62944 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_4_4_2.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<4, 4, 2>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_4_4_3.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_4_4_3.cc new file mode 100644 index 00000000000..52259fb1a67 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_4_4_3.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<4, 4, 3>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_4_4_4.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_4_4_4.cc new file mode 100644 index 00000000000..775424e6c8f --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_4_4_4.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<4, 4, 4>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_4_4_d.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_4_4_d.cc new file mode 100644 index 00000000000..97cde594059 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_4_4_d.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<4, 4, Dynamic>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_d_d_d.cc b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_d_d_d.cc new file mode 100644 index 00000000000..4cba32e26c8 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/generated/schur_eliminator_d_d_d.cc @@ -0,0 +1,53 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Template specialization of SchurEliminator. +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_eliminator_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/schur_eliminator_impl.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +template class SchurEliminator<Dynamic, Dynamic, Dynamic>; + +} // namespace internal +} // namespace ceres + diff --git a/extern/libmv/third_party/ceres/internal/ceres/gradient_checking_cost_function.cc b/extern/libmv/third_party/ceres/internal/ceres/gradient_checking_cost_function.cc new file mode 100644 index 00000000000..abba40824ef --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/gradient_checking_cost_function.cc @@ -0,0 +1,308 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#include "ceres/gradient_checking_cost_function.h" + +#include <algorithm> +#include <cmath> +#include <numeric> +#include <string> +#include <vector> + +#include <glog/logging.h> +#include "ceres/parameter_block.h" +#include "ceres/problem_impl.h" +#include "ceres/program.h" +#include "ceres/residual_block.h" +#include "ceres/runtime_numeric_diff_cost_function.h" +#include "ceres/stringprintf.h" +#include "ceres/cost_function.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/problem.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { +namespace { + +// True if x and y have an absolute relative difference less than +// relative_precision and false otherwise. Stores the relative and absolute +// difference in relative/absolute_error if non-NULL. +bool IsClose(double x, double y, double relative_precision, + double *relative_error, + double *absolute_error) { + double local_absolute_error; + double local_relative_error; + if (!absolute_error) { + absolute_error = &local_absolute_error; + } + if (!relative_error) { + relative_error = &local_relative_error; + } + *absolute_error = fabs(x - y); + *relative_error = *absolute_error / max(fabs(x), fabs(y)); + if (x == 0 || y == 0) { + // If x or y is exactly zero, then relative difference doesn't have any + // meaning. Take the absolute difference instead. + *relative_error = *absolute_error; + } + return fabs(*relative_error) < fabs(relative_precision); +} + +class GradientCheckingCostFunction : public CostFunction { + public: + GradientCheckingCostFunction(const CostFunction* function, + double relative_step_size, + double relative_precision, + const string& extra_info) + : function_(function), + finite_diff_cost_function_( + CreateRuntimeNumericDiffCostFunction(function, + CENTRAL, + relative_step_size)), + relative_precision_(relative_precision), + extra_info_(extra_info) { + *mutable_parameter_block_sizes() = function->parameter_block_sizes(); + set_num_residuals(function->num_residuals()); + } + + virtual ~GradientCheckingCostFunction() { } + + virtual bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const { + if (!jacobians) { + // Nothing to check in this case; just forward. + return function_->Evaluate(parameters, residuals, NULL); + } + + int num_residuals = function_->num_residuals(); + + // Make space for the jacobians of the two methods. + const vector<int16>& block_sizes = function_->parameter_block_sizes(); + vector<Matrix> term_jacobians(block_sizes.size()); + vector<Matrix> finite_difference_jacobians(block_sizes.size()); + vector<double*> term_jacobian_pointers(block_sizes.size()); + vector<double*> finite_difference_jacobian_pointers(block_sizes.size()); + for (int i = 0; i < block_sizes.size(); i++) { + term_jacobians[i].resize(num_residuals, block_sizes[i]); + term_jacobian_pointers[i] = term_jacobians[i].data(); + finite_difference_jacobians[i].resize(num_residuals, block_sizes[i]); + finite_difference_jacobian_pointers[i] = + finite_difference_jacobians[i].data(); + } + + // Evaluate the derivative using the user supplied code. + if (!function_->Evaluate(parameters, + residuals, + &term_jacobian_pointers[0])) { + LOG(WARNING) << "Function evaluation failed."; + return false; + } + + // Evaluate the derivative using numeric derivatives. + finite_diff_cost_function_->Evaluate( + parameters, + residuals, + &finite_difference_jacobian_pointers[0]); + + // See if any elements have relative error larger than the threshold. + int num_bad_jacobian_components = 0; + double worst_relative_error = 0; + + // Accumulate the error message for all the jacobians, since it won't get + // output if there are no bad jacobian components. + string m; + for (int k = 0; k < block_sizes.size(); k++) { + // Copy the original jacobian blocks into the jacobians array. + if (jacobians[k] != NULL) { + MatrixRef(jacobians[k], + term_jacobians[k].rows(), + term_jacobians[k].cols()) = term_jacobians[k]; + } + + StringAppendF(&m, + "========== " + "Jacobian for " "block %d: (%ld by %ld)) " + "==========\n", + k, + term_jacobians[k].rows(), + term_jacobians[k].cols()); + // The funny spacing creates appropriately aligned column headers. + m += " block row col user dx/dy num diff dx/dy " + "abs error relative error parameter residual\n"; + + for (int i = 0; i < term_jacobians[k].rows(); i++) { + for (int j = 0; j < term_jacobians[k].cols(); j++) { + double term_jacobian = term_jacobians[k](i, j); + double finite_jacobian = finite_difference_jacobians[k](i, j); + double relative_error, absolute_error; + bool bad_jacobian_entry = + !IsClose(term_jacobian, + finite_jacobian, + relative_precision_, + &relative_error, + &absolute_error); + worst_relative_error = std::max(worst_relative_error, + relative_error); + + StringAppendF(&m, "%6d %4d %4d %17g %17g %17g %17g %17g %17g", + k, i, j, + term_jacobian, finite_jacobian, + absolute_error, relative_error, + parameters[k][j], + residuals[i]); + + if (bad_jacobian_entry) { + num_bad_jacobian_components++; + StringAppendF( + &m, " ------ (%d,%d,%d) Relative error worse than %g", + k, i, j, relative_precision_); + } + m += "\n"; + } + } + } + + // Since there were some bad errors, dump comprehensive debug info. + if (num_bad_jacobian_components) { + string header = StringPrintf("Detected %d bad jacobian component(s). " + "Worst relative error was %g.\n", + num_bad_jacobian_components, + worst_relative_error); + if (!extra_info_.empty()) { + header += "Extra info for this residual: " + extra_info_ + "\n"; + } + LOG(WARNING) << "\n" << header << m; + } + return true; + } + + private: + const CostFunction* function_; + internal::scoped_ptr<CostFunction> finite_diff_cost_function_; + double relative_precision_; + string extra_info_; +}; + +} // namespace + +CostFunction *CreateGradientCheckingCostFunction( + const CostFunction *cost_function, + double relative_step_size, + double relative_precision, + const string& extra_info) { + return new GradientCheckingCostFunction(cost_function, + relative_step_size, + relative_precision, + extra_info); +} + +ProblemImpl* CreateGradientCheckingProblemImpl(ProblemImpl* problem_impl, + double relative_step_size, + double relative_precision) { + // We create new CostFunctions by wrapping the original CostFunction + // in a gradient checking CostFunction. So its okay for the + // ProblemImpl to take ownership of it and destroy it. The + // LossFunctions and LocalParameterizations are reused and since + // they are owned by problem_impl, gradient_checking_problem_impl + // should not take ownership of it. + Problem::Options gradient_checking_problem_options; + gradient_checking_problem_options.cost_function_ownership = TAKE_OWNERSHIP; + gradient_checking_problem_options.loss_function_ownership = + DO_NOT_TAKE_OWNERSHIP; + gradient_checking_problem_options.local_parameterization_ownership = + DO_NOT_TAKE_OWNERSHIP; + + ProblemImpl* gradient_checking_problem_impl = new ProblemImpl( + gradient_checking_problem_options); + + Program* program = problem_impl->mutable_program(); + + // For every ParameterBlock in problem_impl, create a new parameter + // block with the same local parameterization and constancy. + const vector<ParameterBlock*>& parameter_blocks = program->parameter_blocks(); + for (int i = 0; i < parameter_blocks.size(); ++i) { + ParameterBlock* parameter_block = parameter_blocks[i]; + gradient_checking_problem_impl->AddParameterBlock( + parameter_block->mutable_user_state(), + parameter_block->Size(), + parameter_block->mutable_local_parameterization()); + + if (parameter_block->IsConstant()) { + gradient_checking_problem_impl->SetParameterBlockConstant( + parameter_block->mutable_user_state()); + } + } + + // For every ResidualBlock in problem_impl, create a new + // ResidualBlock by wrapping its CostFunction inside a + // GradientCheckingCostFunction. + const vector<ResidualBlock*>& residual_blocks = program->residual_blocks(); + for (int i = 0; i < residual_blocks.size(); ++i) { + ResidualBlock* residual_block = residual_blocks[i]; + + // Build a human readable string which identifies the + // ResidualBlock. This is used by the GradientCheckingCostFunction + // when logging debugging information. + string extra_info = StringPrintf( + "Residual block id %d; depends on parameters [", i); + vector<double*> parameter_blocks; + for (int j = 0; j < residual_block->NumParameterBlocks(); ++j) { + ParameterBlock* parameter_block = residual_block->parameter_blocks()[j]; + parameter_blocks.push_back(parameter_block->mutable_user_state()); + StringAppendF(&extra_info, "%p", parameter_block->mutable_user_state()); + extra_info += (j < residual_block->NumParameterBlocks() - 1) ? ", " : "]"; + } + + // Wrap the original CostFunction in a GradientCheckingCostFunction. + CostFunction* gradient_checking_cost_function = + CreateGradientCheckingCostFunction(residual_block->cost_function(), + relative_step_size, + relative_precision, + extra_info); + + // The const_cast is necessary because + // ProblemImpl::AddResidualBlock can potentially take ownership of + // the LossFunction, but in this case we are guaranteed that this + // will not be the case, so this const_cast is harmless. + gradient_checking_problem_impl->AddResidualBlock( + gradient_checking_cost_function, + const_cast<LossFunction*>(residual_block->loss_function()), + parameter_blocks); + } + + return gradient_checking_problem_impl; +} + + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/gradient_checking_cost_function.h b/extern/libmv/third_party/ceres/internal/ceres/gradient_checking_cost_function.h new file mode 100644 index 00000000000..d49c8e6c244 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/gradient_checking_cost_function.h @@ -0,0 +1,85 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#ifndef CERES_INTERNAL_GRADIENT_CHECKING_COST_FUNCTION_H_ +#define CERES_INTERNAL_GRADIENT_CHECKING_COST_FUNCTION_H_ + +#include <string> + +#include "ceres/cost_function.h" + +namespace ceres { +namespace internal { + +class ProblemImpl; + +// Creates a CostFunction that checks the jacobians that cost_function computes +// with finite differences. Bad results are logged; required precision is +// controlled by relative_precision and the numeric differentiation step size is +// controlled with relative_step_size. See solver.h for a better explanation of +// relative_step_size. Caller owns result. +// +// The condition enforced is that +// +// (J_actual(i, j) - J_numeric(i, j)) +// ------------------------------------ < relative_precision +// max(J_actual(i, j), J_numeric(i, j)) +// +// where J_actual(i, j) is the jacobian as computed by the supplied cost +// function (by the user) and J_numeric is the jacobian as computed by finite +// differences. +// +// Note: This is quite inefficient and is intended only for debugging. +CostFunction* CreateGradientCheckingCostFunction( + const CostFunction* cost_function, + double relative_step_size, + double relative_precision, + const string& extra_info); + +// Create a new ProblemImpl object from the input problem_impl, where +// each CostFunctions in problem_impl are wrapped inside a +// GradientCheckingCostFunctions. This gives us a ProblemImpl object +// which checks its derivatives against estimates from numeric +// differentiation everytime a ResidualBlock is evaluated. +// +// relative_step_size and relative_precision are parameters to control +// the numeric differentiation and the relative tolerance between the +// jacobian computed by the CostFunctions in problem_impl and +// jacobians obtained by numerically differentiating them. For more +// details see the documentation for +// CreateGradientCheckingCostFunction above. +ProblemImpl* CreateGradientCheckingProblemImpl(ProblemImpl* problem_impl, + double relative_step_size, + double relative_precision); + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_GRADIENT_CHECKING_COST_FUNCTION_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/graph.h b/extern/libmv/third_party/ceres/internal/ceres/graph.h new file mode 100644 index 00000000000..fd7a224f0aa --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/graph.h @@ -0,0 +1,138 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_GRAPH_H_ +#define CERES_INTERNAL_GRAPH_H_ + +#include <limits> +#include <glog/logging.h> +#include "ceres/integral_types.h" +#include "ceres/map_util.h" +#include "ceres/collections_port.h" +#include "ceres/internal/macros.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +// A weighted undirected graph templated over the vertex ids. Vertex +// should be hashable and comparable. +template <typename Vertex> +class Graph { + public: + Graph() {} + + // Add a weighted vertex. If the vertex already exists in the graph, + // its weight is set to the new weight. + void AddVertex(const Vertex& vertex, double weight) { + if (vertices_.find(vertex) == vertices_.end()) { + vertices_.insert(vertex); + edges_[vertex] = HashSet<Vertex>(); + } + vertex_weights_[vertex] = weight; + } + + // Uses weight = 1.0. If vertex already exists, its weight is set to + // 1.0. + void AddVertex(const Vertex& vertex) { + AddVertex(vertex, 1.0); + } + + // Add a weighted edge between the vertex1 and vertex2. Calling + // AddEdge on a pair of vertices which do not exist in the graph yet + // will result in undefined behavior. + // + // It is legal to call this method repeatedly for the same set of + // vertices. + void AddEdge(const Vertex& vertex1, const Vertex& vertex2, double weight) { + DCHECK(vertices_.find(vertex1) != vertices_.end()); + DCHECK(vertices_.find(vertex2) != vertices_.end()); + + if (edges_[vertex1].insert(vertex2).second) { + edges_[vertex2].insert(vertex1); + } + + if (vertex1 < vertex2) { + edge_weights_[make_pair(vertex1, vertex2)] = weight; + } else { + edge_weights_[make_pair(vertex2, vertex1)] = weight; + } + } + + // Uses weight = 1.0. + void AddEdge(const Vertex& vertex1, const Vertex& vertex2) { + AddEdge(vertex1, vertex2, 1.0); + } + + // Calling VertexWeight on a vertex not in the graph will result in + // undefined behavior. + double VertexWeight(const Vertex& vertex) const { + return FindOrDie(vertex_weights_, vertex); + } + + // Calling EdgeWeight on a pair of vertices where either one of the + // vertices is not present in the graph will result in undefined + // behaviour. If there is no edge connecting vertex1 and vertex2, + // the edge weight is zero. + double EdgeWeight(const Vertex& vertex1, const Vertex& vertex2) const { + if (vertex1 < vertex2) { + return FindWithDefault(edge_weights_, make_pair(vertex1, vertex2), 0.0); + } else { + return FindWithDefault(edge_weights_, make_pair(vertex2, vertex1), 0.0); + } + } + + // Calling Neighbors on a vertex not in the graph will result in + // undefined behaviour. + const HashSet<Vertex>& Neighbors(const Vertex& vertex) const { + return FindOrDie(edges_, vertex); + } + + const HashSet<Vertex>& vertices() const { + return vertices_; + } + + static double InvalidWeight() { + return std::numeric_limits<double>::quiet_NaN(); + }; + + private: + HashSet<Vertex> vertices_; + HashMap<Vertex, double> vertex_weights_; + HashMap<Vertex, HashSet<Vertex> > edges_; + HashMap<pair<Vertex, Vertex>, double> edge_weights_; + + DISALLOW_COPY_AND_ASSIGN(Graph); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_GRAPH_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/graph_algorithms.h b/extern/libmv/third_party/ceres/internal/ceres/graph_algorithms.h new file mode 100644 index 00000000000..3b42d936336 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/graph_algorithms.h @@ -0,0 +1,270 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Various algorithms that operate on undirected graphs. + +#ifndef CERES_INTERNAL_GRAPH_ALGORITHMS_H_ +#define CERES_INTERNAL_GRAPH_ALGORITHMS_H_ + +#include <vector> +#include <glog/logging.h> +#include "ceres/collections_port.h" +#include "ceres/graph.h" + +namespace ceres { +namespace internal { + +// Compare two vertices of a graph by their degrees. +template <typename Vertex> +class VertexDegreeLessThan { + public: + explicit VertexDegreeLessThan(const Graph<Vertex>& graph) + : graph_(graph) {} + + bool operator()(const Vertex& lhs, const Vertex& rhs) const { + if (graph_.Neighbors(lhs).size() == graph_.Neighbors(rhs).size()) { + return lhs < rhs; + } + return graph_.Neighbors(lhs).size() < graph_.Neighbors(rhs).size(); + } + + private: + const Graph<Vertex>& graph_; +}; + +// Order the vertices of a graph using its (approximately) largest +// independent set, where an independent set of a graph is a set of +// vertices that have no edges connecting them. The maximum +// independent set problem is NP-Hard, but there are effective +// approximation algorithms available. The implementation here uses a +// breadth first search that explores the vertices in order of +// increasing degree. The same idea is used by Saad & Li in "MIQR: A +// multilevel incomplete QR preconditioner for large sparse +// least-squares problems", SIMAX, 2007. +// +// Given a undirected graph G(V,E), the algorithm is a greedy BFS +// search where the vertices are explored in increasing order of their +// degree. The output vector ordering contains elements of S in +// increasing order of their degree, followed by elements of V - S in +// increasing order of degree. The return value of the function is the +// cardinality of S. +template <typename Vertex> +int IndependentSetOrdering(const Graph<Vertex>& graph, + vector<Vertex>* ordering) { + const HashSet<Vertex>& vertices = graph.vertices(); + const int num_vertices = vertices.size(); + + CHECK_NOTNULL(ordering); + ordering->clear(); + ordering->reserve(num_vertices); + + // Colors for labeling the graph during the BFS. + const char kWhite = 0; + const char kGrey = 1; + const char kBlack = 2; + + // Mark all vertices white. + HashMap<Vertex, char> vertex_color; + vector<Vertex> vertex_queue; + for (typename HashSet<Vertex>::const_iterator it = vertices.begin(); + it != vertices.end(); + ++it) { + vertex_color[*it] = kWhite; + vertex_queue.push_back(*it); + } + + + sort(vertex_queue.begin(), vertex_queue.end(), + VertexDegreeLessThan<Vertex>(graph)); + + // Iterate over vertex_queue. Pick the first white vertex, add it + // to the independent set. Mark it black and its neighbors grey. + for (int i = 0; i < vertex_queue.size(); ++i) { + const Vertex& vertex = vertex_queue[i]; + if (vertex_color[vertex] != kWhite) { + continue; + } + + ordering->push_back(vertex); + vertex_color[vertex] = kBlack; + const HashSet<Vertex>& neighbors = graph.Neighbors(vertex); + for (typename HashSet<Vertex>::const_iterator it = neighbors.begin(); + it != neighbors.end(); + ++it) { + vertex_color[*it] = kGrey; + } + } + + int independent_set_size = ordering->size(); + + // Iterate over the vertices and add all the grey vertices to the + // ordering. At this stage there should only be black or grey + // vertices in the graph. + for (typename vector<Vertex>::const_iterator it = vertex_queue.begin(); + it != vertex_queue.end(); + ++it) { + const Vertex vertex = *it; + DCHECK(vertex_color[vertex] != kWhite); + if (vertex_color[vertex] != kBlack) { + ordering->push_back(vertex); + } + } + + CHECK_EQ(ordering->size(), num_vertices); + return independent_set_size; +} + +// Find the connected component for a vertex implemented using the +// find and update operation for disjoint-set. Recursively traverse +// the disjoint set structure till you reach a vertex whose connected +// component has the same id as the vertex itself. Along the way +// update the connected components of all the vertices. This updating +// is what gives this data structure its efficiency. +template <typename Vertex> +Vertex FindConnectedComponent(const Vertex& vertex, + HashMap<Vertex, Vertex>* union_find) { + typename HashMap<Vertex, Vertex>::iterator it = union_find->find(vertex); + DCHECK(it != union_find->end()); + if (it->second != vertex) { + it->second = FindConnectedComponent(it->second, union_find); + } + + return it->second; +} + +// Compute a degree two constrained Maximum Spanning Tree/forest of +// the input graph. Caller owns the result. +// +// Finding degree 2 spanning tree of a graph is not always +// possible. For example a star graph, i.e. a graph with n-nodes +// where one node is connected to the other n-1 nodes does not have +// a any spanning trees of degree less than n-1.Even if such a tree +// exists, finding such a tree is NP-Hard. + +// We get around both of these problems by using a greedy, degree +// constrained variant of Kruskal's algorithm. We start with a graph +// G_T with the same vertex set V as the input graph G(V,E) but an +// empty edge set. We then iterate over the edges of G in decreasing +// order of weight, adding them to G_T if doing so does not create a +// cycle in G_T} and the degree of all the vertices in G_T remains +// bounded by two. This O(|E|) algorithm results in a degree-2 +// spanning forest, or a collection of linear paths that span the +// graph G. +template <typename Vertex> +Graph<Vertex>* +Degree2MaximumSpanningForest(const Graph<Vertex>& graph) { + // Array of edges sorted in decreasing order of their weights. + vector<pair<double, pair<Vertex, Vertex> > > weighted_edges; + Graph<Vertex>* forest = new Graph<Vertex>(); + + // Disjoint-set to keep track of the connected components in the + // maximum spanning tree. + HashMap<Vertex, Vertex> disjoint_set; + + // Sort of the edges in the graph in decreasing order of their + // weight. Also add the vertices of the graph to the Maximum + // Spanning Tree graph and set each vertex to be its own connected + // component in the disjoint_set structure. + const HashSet<Vertex>& vertices = graph.vertices(); + for (typename HashSet<Vertex>::const_iterator it = vertices.begin(); + it != vertices.end(); + ++it) { + const Vertex vertex1 = *it; + forest->AddVertex(vertex1, graph.VertexWeight(vertex1)); + disjoint_set[vertex1] = vertex1; + + const HashSet<Vertex>& neighbors = graph.Neighbors(vertex1); + for (typename HashSet<Vertex>::const_iterator it2 = neighbors.begin(); + it2 != neighbors.end(); + ++it2) { + const Vertex vertex2 = *it2; + if (vertex1 >= vertex2) { + continue; + } + const double weight = graph.EdgeWeight(vertex1, vertex2); + weighted_edges.push_back(make_pair(weight, make_pair(vertex1, vertex2))); + } + } + + // The elements of this vector, are pairs<edge_weight, + // edge>. Sorting it using the reverse iterators gives us the edges + // in decreasing order of edges. + sort(weighted_edges.rbegin(), weighted_edges.rend()); + + // Greedily add edges to the spanning tree/forest as long as they do + // not violate the degree/cycle constraint. + for (int i =0; i < weighted_edges.size(); ++i) { + const pair<Vertex, Vertex>& edge = weighted_edges[i].second; + const Vertex vertex1 = edge.first; + const Vertex vertex2 = edge.second; + + // Check if either of the vertices are of degree 2 already, in + // which case adding this edge will violate the degree 2 + // constraint. + if ((forest->Neighbors(vertex1).size() == 2) || + (forest->Neighbors(vertex2).size() == 2)) { + continue; + } + + // Find the id of the connected component to which the two + // vertices belong to. If the id is the same, it means that the + // two of them are already connected to each other via some other + // vertex, and adding this edge will create a cycle. + Vertex root1 = FindConnectedComponent(vertex1, &disjoint_set); + Vertex root2 = FindConnectedComponent(vertex2, &disjoint_set); + + if (root1 == root2) { + continue; + } + + // This edge can be added, add an edge in either direction with + // the same weight as the original graph. + const double edge_weight = graph.EdgeWeight(vertex1, vertex2); + forest->AddEdge(vertex1, vertex2, edge_weight); + forest->AddEdge(vertex2, vertex1, edge_weight); + + // Connected the two connected components by updating the + // disjoint_set structure. Always connect the connected component + // with the greater index with the connected component with the + // smaller index. This should ensure shallower trees, for quicker + // lookup. + if (root2 < root1) { + std::swap(root1, root2); + }; + + disjoint_set[root2] = root1; + } + return forest; +} + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_GRAPH_ALGORITHMS_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/implicit_schur_complement.cc b/extern/libmv/third_party/ceres/internal/ceres/implicit_schur_complement.cc new file mode 100644 index 00000000000..bd908846362 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/implicit_schur_complement.cc @@ -0,0 +1,237 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/implicit_schur_complement.h" + +#include <glog/logging.h> +#include "Eigen/Dense" +#include "ceres/block_sparse_matrix.h" +#include "ceres/block_structure.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +ImplicitSchurComplement::ImplicitSchurComplement(int num_eliminate_blocks, + bool constant_sparsity, + bool preconditioner) + : num_eliminate_blocks_(num_eliminate_blocks), + constant_sparsity_(constant_sparsity), + preconditioner_(preconditioner), + A_(NULL), + D_(NULL), + b_(NULL), + block_diagonal_EtE_inverse_(NULL), + block_diagonal_FtF_inverse_(NULL) { +} + +ImplicitSchurComplement::~ImplicitSchurComplement() { +} + +void ImplicitSchurComplement::Init(const BlockSparseMatrixBase& A, + const double* D, + const double* b) { + // Since initialization is reasonably heavy, perhaps we can save on + // constructing a new object everytime. + if ((A_ == NULL) || !constant_sparsity_) { + A_.reset(new PartitionedMatrixView(A, num_eliminate_blocks_)); + } + + D_ = D; + b_ = b; + + // Initialize temporary storage and compute the block diagonals of + // E'E and F'E. + if ((!constant_sparsity_) || (block_diagonal_EtE_inverse_ == NULL)) { + block_diagonal_EtE_inverse_.reset(A_->CreateBlockDiagonalEtE()); + if (preconditioner_) { + block_diagonal_FtF_inverse_.reset(A_->CreateBlockDiagonalFtF()); + } + rhs_.resize(A_->num_cols_f()); + rhs_.setZero(); + tmp_rows_.resize(A_->num_rows()); + tmp_e_cols_.resize(A_->num_cols_e()); + tmp_e_cols_2_.resize(A_->num_cols_e()); + tmp_f_cols_.resize(A_->num_cols_f()); + } else { + A_->UpdateBlockDiagonalEtE(block_diagonal_EtE_inverse_.get()); + if (preconditioner_) { + A_->UpdateBlockDiagonalFtF(block_diagonal_FtF_inverse_.get()); + } + } + + // The block diagonals of the augmented linear system contain + // contributions from the diagonal D if it is non-null. Add that to + // the block diagonals and invert them. + if (D_ != NULL) { + AddDiagonalAndInvert(D_, block_diagonal_EtE_inverse_.get()); + if (preconditioner_) { + AddDiagonalAndInvert(D_ + A_->num_cols_e(), + block_diagonal_FtF_inverse_.get()); + } + } else { + AddDiagonalAndInvert(NULL, block_diagonal_EtE_inverse_.get()); + if (preconditioner_) { + AddDiagonalAndInvert(NULL, block_diagonal_FtF_inverse_.get()); + } + } + + // Compute the RHS of the Schur complement system. + UpdateRhs(); +} + +// Evaluate the product +// +// Sx = [F'F - F'E (E'E)^-1 E'F]x +// +// By breaking it down into individual matrix vector products +// involving the matrices E and F. This is implemented using a +// PartitionedMatrixView of the input matrix A. +void ImplicitSchurComplement::RightMultiply(const double* x, double* y) const { + // y1 = F x + tmp_rows_.setZero(); + A_->RightMultiplyF(x, tmp_rows_.data()); + + // y2 = E' y1 + tmp_e_cols_.setZero(); + A_->LeftMultiplyE(tmp_rows_.data(), tmp_e_cols_.data()); + + // y3 = -(E'E)^-1 y2 + tmp_e_cols_2_.setZero(); + block_diagonal_EtE_inverse_->RightMultiply(tmp_e_cols_.data(), + tmp_e_cols_2_.data()); + tmp_e_cols_2_ *= -1.0; + + // y1 = y1 + E y3 + A_->RightMultiplyE(tmp_e_cols_2_.data(), tmp_rows_.data()); + + // y5 = D * x + if (D_ != NULL) { + ConstVectorRef Dref(D_ + A_->num_cols_e(), num_cols()); + VectorRef(y, num_cols()) = + (Dref.array().square() * + ConstVectorRef(x, num_cols()).array()).matrix(); + } else { + VectorRef(y, num_cols()).setZero(); + } + + // y = y5 + F' y1 + A_->LeftMultiplyF(tmp_rows_.data(), y); +} + +// Given a block diagonal matrix and an optional array of diagonal +// entries D, add them to the diagonal of the matrix and compute the +// inverse of each diagonal block. +void ImplicitSchurComplement::AddDiagonalAndInvert( + const double* D, + BlockSparseMatrix* block_diagonal) { + const CompressedRowBlockStructure* block_diagonal_structure = + block_diagonal->block_structure(); + for (int r = 0; r < block_diagonal_structure->rows.size(); ++r) { + const int row_block_pos = block_diagonal_structure->rows[r].block.position; + const int row_block_size = block_diagonal_structure->rows[r].block.size; + const Cell& cell = block_diagonal_structure->rows[r].cells[0]; + MatrixRef m(block_diagonal->mutable_values() + cell.position, + row_block_size, row_block_size); + + if (D != NULL) { + ConstVectorRef d(D + row_block_pos, row_block_size); + m += d.array().square().matrix().asDiagonal(); + } + + m = m + .selfadjointView<Eigen::Upper>() + .ldlt() + .solve(Matrix::Identity(row_block_size, row_block_size)); + } +} + +// Similar to RightMultiply, use the block structure of the matrix A +// to compute y = (E'E)^-1 (E'b - E'F x). +void ImplicitSchurComplement::BackSubstitute(const double* x, double* y) { + const int num_cols_e = A_->num_cols_e(); + const int num_cols_f = A_->num_cols_f(); + const int num_cols = A_->num_cols(); + const int num_rows = A_->num_rows(); + + // y1 = F x + tmp_rows_.setZero(); + A_->RightMultiplyF(x, tmp_rows_.data()); + + // y2 = b - y1 + tmp_rows_ = ConstVectorRef(b_, num_rows) - tmp_rows_; + + // y3 = E' y2 + tmp_e_cols_.setZero(); + A_->LeftMultiplyE(tmp_rows_.data(), tmp_e_cols_.data()); + + // y = (E'E)^-1 y3 + VectorRef(y, num_cols).setZero(); + block_diagonal_EtE_inverse_->RightMultiply(tmp_e_cols_.data(), y); + + // The full solution vector y has two blocks. The first block of + // variables corresponds to the eliminated variables, which we just + // computed via back substitution. The second block of variables + // corresponds to the Schur complement system, so we just copy those + // values from the solution to the Schur complement. + VectorRef(y + num_cols_e, num_cols_f) = ConstVectorRef(x, num_cols_f); +} + +// Compute the RHS of the Schur complement system. +// +// rhs = F'b - F'E (E'E)^-1 E'b +// +// Like BackSubstitute, we use the block structure of A to implement +// this using a series of matrix vector products. +void ImplicitSchurComplement::UpdateRhs() { + // y1 = E'b + tmp_e_cols_.setZero(); + A_->LeftMultiplyE(b_, tmp_e_cols_.data()); + + // y2 = (E'E)^-1 y1 + Vector y2 = Vector::Zero(A_->num_cols_e()); + block_diagonal_EtE_inverse_->RightMultiply(tmp_e_cols_.data(), y2.data()); + + // y3 = E y2 + tmp_rows_.setZero(); + A_->RightMultiplyE(y2.data(), tmp_rows_.data()); + + // y3 = b - y3 + tmp_rows_ = ConstVectorRef(b_, A_->num_rows()) - tmp_rows_; + + // rhs = F' y3 + rhs_.setZero(); + A_->LeftMultiplyF(tmp_rows_.data(), rhs_.data()); +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/implicit_schur_complement.h b/extern/libmv/third_party/ceres/internal/ceres/implicit_schur_complement.h new file mode 100644 index 00000000000..37a319f9c57 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/implicit_schur_complement.h @@ -0,0 +1,176 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// An iterative solver for solving the Schur complement/reduced camera +// linear system that arise in SfM problems. + +#ifndef CERES_INTERNAL_IMPLICIT_SCHUR_COMPLEMENT_H_ +#define CERES_INTERNAL_IMPLICIT_SCHUR_COMPLEMENT_H_ + +#include "ceres/linear_operator.h" +#include "ceres/partitioned_matrix_view.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +class BlockSparseMatrix; +class BlockSparseMatrixBase; + +// This class implements various linear algebraic operations related +// to the Schur complement without explicitly forming it. +// +// +// Given a reactangular linear system Ax = b, where +// +// A = [E F] +// +// The normal equations are given by +// +// A'Ax = A'b +// +// |E'E E'F||y| = |E'b| +// |F'E F'F||z| |F'b| +// +// and the Schur complement system is given by +// +// [F'F - F'E (E'E)^-1 E'F] z = F'b - F'E (E'E)^-1 E'b +// +// Now if we wish to solve Ax = b in the least squares sense, one way +// is to form this Schur complement system and solve it using +// Preconditioned Conjugate Gradients. +// +// The key operation in a conjugate gradient solver is the evaluation of the +// matrix vector product with the Schur complement +// +// S = F'F - F'E (E'E)^-1 E'F +// +// It is straightforward to see that matrix vector products with S can +// be evaluated without storing S in memory. Instead, given (E'E)^-1 +// (which for our purposes is an easily inverted block diagonal +// matrix), it can be done in terms of matrix vector products with E, +// F and (E'E)^-1. This class implements this functionality and other +// auxilliary bits needed to implement a CG solver on the Schur +// complement using the PartitionedMatrixView object. +// +// THREAD SAFETY: This class is nqot thread safe. In particular, the +// RightMultiply (and the LeftMultiply) methods are not thread safe as +// they depend on mutable arrays used for the temporaries needed to +// compute the product y += Sx; +class ImplicitSchurComplement : public LinearOperator { + public: + // num_eliminate_blocks is the number of E blocks in the matrix + // A. + // + // constant_sparsity indicates if across calls to Init, the sparsity + // structure of the matrix A remains constant or not. This makes for + // significant savings across multiple matrices A, e.g. when used in + // conjunction with an optimization algorithm. + // + // preconditioner indicates whether the inverse of the matrix F'F + // should be computed or not as a preconditioner for the Schur + // Complement. + // + // TODO(sameeragarwal): Get rid of the two bools below and replace + // them with enums. + ImplicitSchurComplement(int num_eliminate_blocks, + bool constant_sparsity, + bool preconditioner); + virtual ~ImplicitSchurComplement(); + + // Initialize the Schur complement for a linear least squares + // problem of the form + // + // |A | x = |b| + // |diag(D)| |0| + // + // If D is null, then it is treated as a zero dimensional matrix. It + // is important that the matrix A have a BlockStructure object + // associated with it and has a block structure that is compatible + // with the SchurComplement solver. + void Init(const BlockSparseMatrixBase& A, const double* D, const double* b); + + // y += Sx, where S is the Schur complement. + virtual void RightMultiply(const double* x, double* y) const; + + // The Schur complement is a symmetric positive definite matrix, + // thus the left and right multiply operators are the same. + virtual void LeftMultiply(const double* x, double* y) const { + RightMultiply(x, y); + } + + // y = (E'E)^-1 (E'b - E'F x). Given an estimate of the solution to + // the Schur complement system, this method computes the value of + // the e_block variables that were eliminated to form the Schur + // complement. + void BackSubstitute(const double* x, double* y); + + virtual int num_rows() const { return A_->num_cols_f(); } + virtual int num_cols() const { return A_->num_cols_f(); } + const Vector& rhs() const { return rhs_; } + + const BlockSparseMatrix* block_diagonal_EtE_inverse() const { + return block_diagonal_EtE_inverse_.get(); + } + + const BlockSparseMatrix* block_diagonal_FtF_inverse() const { + return block_diagonal_FtF_inverse_.get(); + } + + private: + void AddDiagonalAndInvert(const double* D, BlockSparseMatrix* matrix); + void UpdateRhs(); + + int num_eliminate_blocks_; + bool constant_sparsity_; + bool preconditioner_; + + scoped_ptr<PartitionedMatrixView> A_; + const double* D_; + const double* b_; + + scoped_ptr<BlockSparseMatrix> block_diagonal_EtE_inverse_; + scoped_ptr<BlockSparseMatrix> block_diagonal_FtF_inverse_; + + Vector rhs_; + + // Temporary storage vectors used to implement RightMultiply. + mutable Vector tmp_rows_; + mutable Vector tmp_e_cols_; + mutable Vector tmp_e_cols_2_; + mutable Vector tmp_f_cols_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_IMPLICIT_SCHUR_COMPLEMENT_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/integral_types.h b/extern/libmv/third_party/ceres/internal/ceres/integral_types.h new file mode 100644 index 00000000000..01e04937e3e --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/integral_types.h @@ -0,0 +1,92 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// Portable typedefs for various fixed-size integers. Uses template +// metaprogramming instead of fragile compiler defines. + +#ifndef CERES_INTERNAL_INTEGRAL_TYPES_H_ +#define CERES_INTERNAL_INTEGRAL_TYPES_H_ + +namespace ceres { +namespace internal { + +// Compile time ternary on types. +template<bool kCondition, typename kTrueType, typename kFalseType> +struct Ternary { + typedef kTrueType type; +}; +template<typename kTrueType, typename kFalseType> +struct Ternary<false, kTrueType, kFalseType> { + typedef kFalseType type; +}; + +#define CERES_INTSIZE(TYPE) \ + typename Ternary<sizeof(TYPE) * 8 == kBits, TYPE, + +template<int kBits> +struct Integer { + typedef + CERES_INTSIZE(char) + CERES_INTSIZE(short) + CERES_INTSIZE(int) + CERES_INTSIZE(long int) + CERES_INTSIZE(long long) + void>::type >::type >::type >::type >::type + type; +}; + +template<int kBits> +struct UnsignedInteger { + typedef + CERES_INTSIZE(unsigned char) + CERES_INTSIZE(unsigned short) + CERES_INTSIZE(unsigned int) + CERES_INTSIZE(unsigned long int) + CERES_INTSIZE(unsigned long long) + void>::type >::type >::type >::type >::type + type; +}; + +#undef CERES_INTSIZE + +typedef Integer< 8>::type int8; +typedef Integer<16>::type int16; +typedef Integer<32>::type int32; +typedef Integer<64>::type int64; + +typedef UnsignedInteger< 8>::type uint8; +typedef UnsignedInteger<16>::type uint16; +typedef UnsignedInteger<32>::type uint32; +typedef UnsignedInteger<64>::type uint64; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_INTEGRAL_TYPES_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/iterative_schur_complement_solver.cc b/extern/libmv/third_party/ceres/internal/ceres/iterative_schur_complement_solver.cc new file mode 100644 index 00000000000..51303195317 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/iterative_schur_complement_solver.cc @@ -0,0 +1,150 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/iterative_schur_complement_solver.h" + +#include <algorithm> +#include <cstring> +#include <vector> + +#include <glog/logging.h> +#include "Eigen/Dense" +#include "ceres/block_sparse_matrix.h" +#include "ceres/block_structure.h" +#include "ceres/conjugate_gradients_solver.h" +#include "ceres/implicit_schur_complement.h" +#include "ceres/linear_solver.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/visibility_based_preconditioner.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/linear_solver.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/types.h" +#include "ceres/visibility_based_preconditioner.h" + +namespace ceres { +namespace internal { + +IterativeSchurComplementSolver::IterativeSchurComplementSolver( + const LinearSolver::Options& options) + : options_(options) { +} + +IterativeSchurComplementSolver::~IterativeSchurComplementSolver() { +} + +LinearSolver::Summary IterativeSchurComplementSolver::SolveImpl( + BlockSparseMatrixBase* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x) { + CHECK_NOTNULL(A->block_structure()); + + // Initialize a ImplicitSchurComplement object. + if ((schur_complement_ == NULL) || (!options_.constant_sparsity)) { + schur_complement_.reset( + new ImplicitSchurComplement(options_.num_eliminate_blocks, + options_.constant_sparsity, + options_.preconditioner_type == JACOBI)); + } + schur_complement_->Init(*A, per_solve_options.D, b); + + // Initialize the solution to the Schur complement system to zero. + // + // TODO(sameeragarwal): There maybe a better initialization than an + // all zeros solution. Explore other cheap starting points. + reduced_linear_system_solution_.resize(schur_complement_->num_rows()); + reduced_linear_system_solution_.setZero(); + + // Instantiate a conjugate gradient solver that runs on the Schur complement + // matrix with the block diagonal of the matrix F'F as the preconditioner. + LinearSolver::Options cg_options; + cg_options.max_num_iterations = options_.max_num_iterations; + ConjugateGradientsSolver cg_solver(cg_options); + LinearSolver::PerSolveOptions cg_per_solve_options; + + cg_per_solve_options.r_tolerance = per_solve_options.r_tolerance; + cg_per_solve_options.q_tolerance = per_solve_options.q_tolerance; + + bool is_preconditioner_good = false; + switch (options_.preconditioner_type) { + case IDENTITY: + is_preconditioner_good = true; + break; + case JACOBI: + // We need to strip the constness of the block_diagonal_FtF_inverse + // matrix here because the only other way to initialize the struct + // cg_solve_options would be to add a constructor to it. We know + // that the only method ever called on the preconditioner is the + // RightMultiply which is a const method so we don't need to worry + // about the object getting modified. + cg_per_solve_options.preconditioner = + const_cast<BlockSparseMatrix*>( + schur_complement_->block_diagonal_FtF_inverse()); + is_preconditioner_good = true; + break; + case SCHUR_JACOBI: + case CLUSTER_JACOBI: + case CLUSTER_TRIDIAGONAL: + if (visibility_based_preconditioner_.get() == NULL) { + visibility_based_preconditioner_.reset( + new VisibilityBasedPreconditioner(*A->block_structure(), options_)); + } + is_preconditioner_good = + visibility_based_preconditioner_->Compute(*A, per_solve_options.D); + cg_per_solve_options.preconditioner = + visibility_based_preconditioner_.get(); + break; + default: + LOG(FATAL) << "Unknown Preconditioner Type"; + } + + LinearSolver::Summary cg_summary; + cg_summary.num_iterations = 0; + cg_summary.termination_type = FAILURE; + + if (is_preconditioner_good) { + cg_summary = cg_solver.Solve(schur_complement_.get(), + schur_complement_->rhs().data(), + cg_per_solve_options, + reduced_linear_system_solution_.data()); + if (cg_summary.termination_type != FAILURE) { + schur_complement_->BackSubstitute( + reduced_linear_system_solution_.data(), x); + } + } + + VLOG(2) << "CG Iterations : " << cg_summary.num_iterations; + return cg_summary; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/iterative_schur_complement_solver.h b/extern/libmv/third_party/ceres/internal/ceres/iterative_schur_complement_solver.h new file mode 100644 index 00000000000..cfeb65e1eec --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/iterative_schur_complement_solver.h @@ -0,0 +1,94 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_ITERATIVE_SCHUR_COMPLEMENT_SOLVER_H_ +#define CERES_INTERNAL_ITERATIVE_SCHUR_COMPLEMENT_SOLVER_H_ + +#include "ceres/linear_solver.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +class BlockSparseMatrix; +class BlockSparseMatrixBase; +class ImplicitSchurComplement; +class VisibilityBasedPreconditioner; + +// This class implements an iterative solver for the linear least +// squares problems that have a bi-partitte sparsity structure common +// to Structure from Motion problems. +// +// The algorithm used by this solver was developed in a series of +// papers - "Agarwal et al, Bundle Adjustment in the Large, ECCV 2010" +// and "Wu et al, Multicore Bundle Adjustment, submitted to CVPR +// 2011" at the Univeristy of Washington. +// +// The key idea is that one can run Conjugate Gradients on the Schur +// Complement system without explicitly forming the Schur Complement +// in memory. The heavy lifting for this is done by the +// ImplicitSchurComplement class. Not forming the Schur complement in +// memory and factoring it results in substantial savings in time and +// memory. Further, iterative solvers like this open up the +// possibility of solving the Newton equations in a non-linear solver +// only approximately and terminating early, thereby saving even more +// time. +// +// For the curious, running CG on the Schur complement is the same as +// running CG on the Normal Equations with an SSOR preconditioner. For +// a proof of this fact and others related to this solver please see +// the section on Domain Decomposition Methods in Saad's book +// "Iterative Methods for Sparse Linear Systems". +class IterativeSchurComplementSolver : public BlockSparseMatrixBaseSolver { + public: + explicit IterativeSchurComplementSolver( + const LinearSolver::Options& options); + + virtual ~IterativeSchurComplementSolver(); + + private: + virtual LinearSolver::Summary SolveImpl( + BlockSparseMatrixBase* A, + const double* b, + const LinearSolver::PerSolveOptions& options, + double* x); + + LinearSolver::Options options_; + scoped_ptr<internal::ImplicitSchurComplement> schur_complement_; + scoped_ptr<VisibilityBasedPreconditioner> visibility_based_preconditioner_; + Vector reduced_linear_system_solution_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_ITERATIVE_SCHUR_COMPLEMENT_SOLVER_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/levenberg_marquardt.cc b/extern/libmv/third_party/ceres/internal/ceres/levenberg_marquardt.cc new file mode 100644 index 00000000000..b40a5162adc --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/levenberg_marquardt.cc @@ -0,0 +1,574 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Implementation of a simple LM algorithm which uses the step sizing +// rule of "Methods for Nonlinear Least Squares" by K. Madsen, +// H.B. Nielsen and O. Tingleff. Available to download from +// +// http://www2.imm.dtu.dk/pubdb/views/edoc_download.php/3215/pdf/imm3215.pdf +// +// The basic algorithm described in this note is an exact step +// algorithm that depends on the Newton(LM) step being solved exactly +// in each iteration. When a suitable iterative solver is available to +// solve the Newton(LM) step, the algorithm will automatically switch +// to an inexact step solution method. This trades some slowdown in +// convergence for significant savings in solve time and memory +// usage. Our implementation of the Truncated Newton algorithm follows +// the discussion and recommendataions in "Stephen G. Nash, A Survey +// of Truncated Newton Methods, Journal of Computational and Applied +// Mathematics, 124(1-2), 45-59, 2000. + +#include "ceres/levenberg_marquardt.h" + +#include <algorithm> +#include <cstdlib> +#include <cmath> +#include <cstring> +#include <string> +#include <vector> + +#include <glog/logging.h> +#include "Eigen/Core" +#include "ceres/evaluator.h" +#include "ceres/file.h" +#include "ceres/linear_least_squares_problems.h" +#include "ceres/linear_solver.h" +#include "ceres/matrix_proto.h" +#include "ceres/sparse_matrix.h" +#include "ceres/stringprintf.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { +namespace { + +// Numbers for clamping the size of the LM diagonal. The size of these +// numbers is heuristic. We will probably be adjusting them in the +// future based on more numerical experience. With jacobi scaling +// enabled, these numbers should be all but redundant. +const double kMinLevenbergMarquardtDiagonal = 1e-6; +const double kMaxLevenbergMarquardtDiagonal = 1e32; + +// Small constant for various floating point issues. +const double kEpsilon = 1e-12; + +// Number of times the linear solver should be retried in case of +// numerical failure. The retries are done by exponentially scaling up +// mu at each retry. This leads to stronger and stronger +// regularization making the linear least squares problem better +// conditioned at each retry. +const int kMaxLinearSolverRetries = 5; + +// D = 1/sqrt(diag(J^T * J)) +void EstimateScale(const SparseMatrix& jacobian, double* D) { + CHECK_NOTNULL(D); + jacobian.SquaredColumnNorm(D); + for (int i = 0; i < jacobian.num_cols(); ++i) { + D[i] = 1.0 / (kEpsilon + sqrt(D[i])); + } +} + +// D = diag(J^T * J) +void LevenbergMarquardtDiagonal(const SparseMatrix& jacobian, + double* D) { + CHECK_NOTNULL(D); + jacobian.SquaredColumnNorm(D); + for (int i = 0; i < jacobian.num_cols(); ++i) { + D[i] = min(max(D[i], kMinLevenbergMarquardtDiagonal), + kMaxLevenbergMarquardtDiagonal); + } +} + +bool RunCallback(IterationCallback* callback, + const IterationSummary& iteration_summary, + Solver::Summary* summary) { + const CallbackReturnType status = (*callback)(iteration_summary); + switch (status) { + case SOLVER_TERMINATE_SUCCESSFULLY: + summary->termination_type = USER_SUCCESS; + VLOG(1) << "Terminating on USER_SUCCESS."; + return false; + case SOLVER_ABORT: + summary->termination_type = USER_ABORT; + VLOG(1) << "Terminating on USER_ABORT."; + return false; + case SOLVER_CONTINUE: + return true; + default: + LOG(FATAL) << "Unknown status returned by callback: " + << status; + return NULL; + } +} + +} // namespace + +LevenbergMarquardt::~LevenbergMarquardt() {} + +void LevenbergMarquardt::Minimize(const Minimizer::Options& options, + Evaluator* evaluator, + LinearSolver* linear_solver, + const double* initial_parameters, + double* final_parameters, + Solver::Summary* summary) { + time_t start_time = time(NULL); + const int num_parameters = evaluator->NumParameters(); + const int num_effective_parameters = evaluator->NumEffectiveParameters(); + const int num_residuals = evaluator->NumResiduals(); + + summary->termination_type = NO_CONVERGENCE; + summary->num_successful_steps = 0; + summary->num_unsuccessful_steps = 0; + + // Allocate the various vectors needed by the algorithm. + memcpy(final_parameters, initial_parameters, + num_parameters * sizeof(*initial_parameters)); + + VectorRef x(final_parameters, num_parameters); + Vector x_new(num_parameters); + + Vector lm_step(num_effective_parameters); + Vector gradient(num_effective_parameters); + Vector scaled_gradient(num_effective_parameters); + // Jacobi scaling vector + Vector scale(num_effective_parameters); + + Vector f_model(num_residuals); + Vector f(num_residuals); + Vector f_new(num_residuals); + Vector D(num_parameters); + Vector muD(num_parameters); + + // Ask the Evaluator to create the jacobian matrix. The sparsity + // pattern of this matrix is going to remain constant, so we only do + // this once and then re-use this matrix for all subsequent Jacobian + // computations. + scoped_ptr<SparseMatrix> jacobian(evaluator->CreateJacobian()); + + double x_norm = x.norm(); + + double cost = 0.0; + D.setOnes(); + f.setZero(); + + // Do initial cost and Jacobian evaluation. + if (!evaluator->Evaluate(x.data(), &cost, f.data(), jacobian.get())) { + LOG(WARNING) << "Failed to compute residuals and Jacobian. " + << "Terminating."; + summary->termination_type = NUMERICAL_FAILURE; + return; + } + + if (options.jacobi_scaling) { + EstimateScale(*jacobian, scale.data()); + jacobian->ScaleColumns(scale.data()); + } else { + scale.setOnes(); + } + + // This is a poor way to do this computation. Even if fixed_cost is + // zero, because we are subtracting two possibly large numbers, we + // are depending on exact cancellation to give us a zero here. But + // initial_cost and cost have been computed by two different + // evaluators. One which runs on the whole problem (in + // solver_impl.cc) in single threaded mode and another which runs + // here on the reduced problem, so fixed_cost can (and does) contain + // some numerical garbage with a relative magnitude of 1e-14. + // + // The right way to do this, would be to compute the fixed cost on + // just the set of residual blocks which are held constant and were + // removed from the original problem when the reduced problem was + // constructed. + summary->fixed_cost = summary->initial_cost - cost; + + double model_cost = f.squaredNorm() / 2.0; + double total_cost = summary->fixed_cost + cost; + + scaled_gradient.setZero(); + jacobian->LeftMultiply(f.data(), scaled_gradient.data()); + gradient = scaled_gradient.array() / scale.array(); + + double gradient_max_norm = gradient.lpNorm<Eigen::Infinity>(); + // We need the max here to guard againt the gradient being zero. + const double gradient_max_norm_0 = max(gradient_max_norm, kEpsilon); + double gradient_tolerance = options.gradient_tolerance * gradient_max_norm_0; + + double mu = options.tau; + double nu = 2.0; + int iteration = 0; + double actual_cost_change = 0.0; + double step_norm = 0.0; + double relative_decrease = 0.0; + + // Insane steps are steps which are not sane, i.e. there is some + // numerical kookiness going on with them. There are various reasons + // for this kookiness, some easier to diagnose then others. From the + // point of view of the non-linear solver, they are steps which + // cannot be used. We return with NUMERICAL_FAILURE after + // kMaxLinearSolverRetries consecutive insane steps. + bool step_is_sane = false; + int num_consecutive_insane_steps = 0; + + // Whether the step resulted in a sufficient decrease in the + // objective function when compared to the decrease in the value of + // the lineariztion. + bool step_is_successful = false; + + // Parse the iterations for which to dump the linear problem. + vector<int> iterations_to_dump = options.lsqp_iterations_to_dump; + sort(iterations_to_dump.begin(), iterations_to_dump.end()); + + IterationSummary iteration_summary; + iteration_summary.iteration = iteration; + iteration_summary.step_is_successful = false; + iteration_summary.cost = total_cost; + iteration_summary.cost_change = actual_cost_change; + iteration_summary.gradient_max_norm = gradient_max_norm; + iteration_summary.step_norm = step_norm; + iteration_summary.relative_decrease = relative_decrease; + iteration_summary.mu = mu; + iteration_summary.eta = options.eta; + iteration_summary.linear_solver_iterations = 0; + iteration_summary.linear_solver_time_sec = 0.0; + iteration_summary.iteration_time_sec = (time(NULL) - start_time); + if (options.logging_type >= PER_MINIMIZER_ITERATION) { + summary->iterations.push_back(iteration_summary); + } + + // Check if the starting point is an optimum. + VLOG(2) << "Gradient max norm: " << gradient_max_norm + << " tolerance: " << gradient_tolerance + << " ratio: " << gradient_max_norm / gradient_max_norm_0 + << " tolerance: " << options.gradient_tolerance; + if (gradient_max_norm <= gradient_tolerance) { + summary->termination_type = GRADIENT_TOLERANCE; + VLOG(1) << "Terminating on GRADIENT_TOLERANCE. " + << "Relative gradient max norm: " + << gradient_max_norm / gradient_max_norm_0 + << " <= " << options.gradient_tolerance; + return; + } + + // Call the various callbacks. + for (int i = 0; i < options.callbacks.size(); ++i) { + if (!RunCallback(options.callbacks[i], iteration_summary, summary)) { + return; + } + } + + // We only need the LM diagonal if we are actually going to do at + // least one iteration of the optimization. So we wait to do it + // until now. + LevenbergMarquardtDiagonal(*jacobian, D.data()); + + while ((iteration < options.max_num_iterations) && + (time(NULL) - start_time) <= options.max_solver_time_sec) { + time_t iteration_start_time = time(NULL); + step_is_sane = false; + step_is_successful = false; + + IterationSummary iteration_summary; + // The while loop here is just to provide an easily breakable + // control structure. We are guaranteed to always exit this loop + // at the end of one iteration or before. + while (1) { + muD = (mu * D).array().sqrt(); + LinearSolver::PerSolveOptions solve_options; + solve_options.D = muD.data(); + solve_options.q_tolerance = options.eta; + // Disable r_tolerance checking. Since we only care about + // termination via the q_tolerance. As Nash and Sofer show, + // r_tolerance based termination is essentially useless in + // Truncated Newton methods. + solve_options.r_tolerance = -1.0; + + const time_t linear_solver_start_time = time(NULL); + LinearSolver::Summary linear_solver_summary = + linear_solver->Solve(jacobian.get(), + f.data(), + solve_options, + lm_step.data()); + iteration_summary.linear_solver_time_sec = + (time(NULL) - linear_solver_start_time); + iteration_summary.linear_solver_iterations = + linear_solver_summary.num_iterations; + + if (binary_search(iterations_to_dump.begin(), + iterations_to_dump.end(), + iteration)) { + CHECK(DumpLinearLeastSquaresProblem(options.lsqp_dump_directory, + iteration, + options.lsqp_dump_format_type, + jacobian.get(), + muD.data(), + f.data(), + lm_step.data(), + options.num_eliminate_blocks)) + << "Tried writing linear least squares problem: " + << options.lsqp_dump_directory + << " but failed."; + } + + // We ignore the case where the linear solver did not converge, + // since the partial solution computed by it still maybe of use, + // and there is no reason to ignore it, especially since we + // spent so much time computing it. + if ((linear_solver_summary.termination_type != TOLERANCE) && + (linear_solver_summary.termination_type != MAX_ITERATIONS)) { + VLOG(1) << "Linear solver failure: retrying with a higher mu"; + break; + } + + step_norm = (lm_step.array() * scale.array()).matrix().norm(); + + // Check step length based convergence. If the step length is + // too small, then we are done. + const double step_size_tolerance = options.parameter_tolerance * + (x_norm + options.parameter_tolerance); + + VLOG(2) << "Step size: " << step_norm + << " tolerance: " << step_size_tolerance + << " ratio: " << step_norm / step_size_tolerance + << " tolerance: " << options.parameter_tolerance; + if (step_norm <= options.parameter_tolerance * + (x_norm + options.parameter_tolerance)) { + summary->termination_type = PARAMETER_TOLERANCE; + VLOG(1) << "Terminating on PARAMETER_TOLERANCE." + << "Relative step size: " << step_norm / step_size_tolerance + << " <= " << options.parameter_tolerance; + return; + } + + Vector delta = -(lm_step.array() * scale.array()).matrix(); + if (!evaluator->Plus(x.data(), delta.data(), x_new.data())) { + LOG(WARNING) << "Failed to compute Plus(x, delta, x_plus_delta). " + << "Terminating."; + summary->termination_type = NUMERICAL_FAILURE; + return; + } + + double cost_new = 0.0; + if (!evaluator->Evaluate(x_new.data(), &cost_new, NULL, NULL)) { + LOG(WARNING) << "Failed to compute the value of the objective " + << "function. Terminating."; + summary->termination_type = NUMERICAL_FAILURE; + return; + } + + f_model.setZero(); + jacobian->RightMultiply(lm_step.data(), f_model.data()); + const double model_cost_new = + (f.segment(0, num_residuals) - f_model).squaredNorm() / 2; + + actual_cost_change = cost - cost_new; + double model_cost_change = model_cost - model_cost_new; + + VLOG(2) << "[Model cost] current: " << model_cost + << " new : " << model_cost_new + << " change: " << model_cost_change; + + VLOG(2) << "[Nonlinear cost] current: " << cost + << " new : " << cost_new + << " change: " << actual_cost_change + << " relative change: " << fabs(actual_cost_change) / cost + << " tolerance: " << options.function_tolerance; + + // In exact arithmetic model_cost_change should never be + // negative. But due to numerical precision issues, we may end up + // with a small negative number. model_cost_change which are + // negative and large in absolute value are indicative of a + // numerical failure in the solver. + if (model_cost_change < -kEpsilon) { + VLOG(1) << "Model cost change is negative.\n" + << "Current : " << model_cost + << " new : " << model_cost_new + << " change: " << model_cost_change << "\n"; + break; + } + + // If we have reached this far, then we are willing to trust the + // numerical quality of the step. + step_is_sane = true; + num_consecutive_insane_steps = 0; + + // Check function value based convergence. + if (fabs(actual_cost_change) < options.function_tolerance * cost) { + VLOG(1) << "Termination on FUNCTION_TOLERANCE." + << " Relative cost change: " << fabs(actual_cost_change) / cost + << " tolerance: " << options.function_tolerance; + summary->termination_type = FUNCTION_TOLERANCE; + return; + } + + // Clamp model_cost_change at kEpsilon from below. + if (model_cost_change < kEpsilon) { + VLOG(1) << "Clamping model cost change " << model_cost_change + << " to " << kEpsilon; + model_cost_change = kEpsilon; + } + + relative_decrease = actual_cost_change / model_cost_change; + VLOG(2) << "actual_cost_change / model_cost_change = " + << relative_decrease; + + if (relative_decrease < options.min_relative_decrease) { + VLOG(2) << "Unsuccessful step."; + break; + } + + VLOG(2) << "Successful step."; + + ++summary->num_successful_steps; + x = x_new; + x_norm = x.norm(); + + if (!evaluator->Evaluate(x.data(), &cost, f.data(), jacobian.get())) { + LOG(WARNING) << "Failed to compute residuals and jacobian. " + << "Terminating."; + summary->termination_type = NUMERICAL_FAILURE; + return; + } + + if (options.jacobi_scaling) { + jacobian->ScaleColumns(scale.data()); + } + + model_cost = f.squaredNorm() / 2.0; + LevenbergMarquardtDiagonal(*jacobian, D.data()); + scaled_gradient.setZero(); + jacobian->LeftMultiply(f.data(), scaled_gradient.data()); + gradient = scaled_gradient.array() / scale.array(); + gradient_max_norm = gradient.lpNorm<Eigen::Infinity>(); + + // Check gradient based convergence. + VLOG(2) << "Gradient max norm: " << gradient_max_norm + << " tolerance: " << gradient_tolerance + << " ratio: " << gradient_max_norm / gradient_max_norm_0 + << " tolerance: " << options.gradient_tolerance; + if (gradient_max_norm <= gradient_tolerance) { + summary->termination_type = GRADIENT_TOLERANCE; + VLOG(1) << "Terminating on GRADIENT_TOLERANCE. " + << "Relative gradient max norm: " + << gradient_max_norm / gradient_max_norm_0 + << " <= " << options.gradient_tolerance + << " (tolerance)."; + return; + } + + mu = mu * max(1.0 / 3.0, 1 - pow(2 * relative_decrease - 1, 3)); + nu = 2.0; + step_is_successful = true; + break; + } + + if (!step_is_sane) { + ++num_consecutive_insane_steps; + } + + if (num_consecutive_insane_steps == kMaxLinearSolverRetries) { + summary->termination_type = NUMERICAL_FAILURE; + VLOG(1) << "Too many consecutive retries; ending with numerical fail."; + + if (!options.crash_and_dump_lsqp_on_failure) { + return; + } + + // Dump debugging information to disk. + CHECK(options.lsqp_dump_format_type == TEXTFILE || + options.lsqp_dump_format_type == PROTOBUF) + << "Dumping the linear least squares problem on crash " + << "requires Solver::Options::lsqp_dump_format_type to be " + << "PROTOBUF or TEXTFILE."; + + if (DumpLinearLeastSquaresProblem(options.lsqp_dump_directory, + iteration, + options.lsqp_dump_format_type, + jacobian.get(), + muD.data(), + f.data(), + lm_step.data(), + options.num_eliminate_blocks)) { + LOG(FATAL) << "Linear least squares problem saved to: " + << options.lsqp_dump_directory + << ". Please provide this to the Ceres developers for " + << " debugging along with the v=2 log."; + } else { + LOG(FATAL) << "Tried writing linear least squares problem: " + << options.lsqp_dump_directory + << " but failed."; + } + } + + if (!step_is_successful) { + // Either the step did not lead to a decrease in cost or there + // was numerical failure. In either case we will scale mu up and + // retry. If it was a numerical failure, we hope that the + // stronger regularization will make the linear system better + // conditioned. If it was numerically sane, but there was no + // decrease in cost, then increasing mu reduces the size of the + // trust region and we look for a decrease closer to the + // linearization point. + ++summary->num_unsuccessful_steps; + mu = mu * nu; + nu = 2 * nu; + } + + ++iteration; + + total_cost = summary->fixed_cost + cost; + + iteration_summary.iteration = iteration; + iteration_summary.step_is_successful = step_is_successful; + iteration_summary.cost = total_cost; + iteration_summary.cost_change = actual_cost_change; + iteration_summary.gradient_max_norm = gradient_max_norm; + iteration_summary.step_norm = step_norm; + iteration_summary.relative_decrease = relative_decrease; + iteration_summary.mu = mu; + iteration_summary.eta = options.eta; + iteration_summary.iteration_time_sec = (time(NULL) - iteration_start_time); + + if (options.logging_type >= PER_MINIMIZER_ITERATION) { + summary->iterations.push_back(iteration_summary); + } + + // Call the various callbacks. + for (int i = 0; i < options.callbacks.size(); ++i) { + if (!RunCallback(options.callbacks[i], iteration_summary, summary)) { + return; + } + } + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/levenberg_marquardt.h b/extern/libmv/third_party/ceres/internal/ceres/levenberg_marquardt.h new file mode 100644 index 00000000000..d00bb9095be --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/levenberg_marquardt.h @@ -0,0 +1,65 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Implmentation of Levenberg Marquardt algorithm based on "Methods for +// Nonlinear Least Squares" by K. Madsen, H.B. Nielsen and +// O. Tingleff. Available to download from +// +// http://www2.imm.dtu.dk/pubdb/views/edoc_download.php/3215/pdf/imm3215.pdf +// + +#ifndef CERES_INTERNAL_LEVENBERG_MARQUARDT_H_ +#define CERES_INTERNAL_LEVENBERG_MARQUARDT_H_ + +#include "ceres/minimizer.h" +#include "ceres/solver.h" + +namespace ceres { +namespace internal { + +class Evaluator; +class LinearSolver; + +class LevenbergMarquardt : public Minimizer { + public: + virtual ~LevenbergMarquardt(); + + virtual void Minimize(const Minimizer::Options& options, + Evaluator* evaluator, + LinearSolver* linear_solver, + const double* initial_parameters, + double* final_parameters, + Solver::Summary* summary); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_LEVENBERG_MARQUARDT_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/linear_least_squares_problems.cc b/extern/libmv/third_party/ceres/internal/ceres/linear_least_squares_problems.cc new file mode 100644 index 00000000000..cca9f442fe7 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/linear_least_squares_problems.cc @@ -0,0 +1,744 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/linear_least_squares_problems.h" + +#include <cstdio> +#include <string> +#include <vector> +#include <glog/logging.h> +#include "ceres/block_sparse_matrix.h" +#include "ceres/block_structure.h" +#include "ceres/casts.h" +#include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/file.h" +#include "ceres/matrix_proto.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/stringprintf.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +LinearLeastSquaresProblem* CreateLinearLeastSquaresProblemFromId(int id) { + switch (id) { + case 0: + return LinearLeastSquaresProblem0(); + case 1: + return LinearLeastSquaresProblem1(); + case 2: + return LinearLeastSquaresProblem2(); + case 3: + return LinearLeastSquaresProblem3(); + default: + LOG(FATAL) << "Unknown problem id requested " << id; + } + return NULL; +} + +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +LinearLeastSquaresProblem* CreateLinearLeastSquaresProblemFromFile( + const string& filename) { + LinearLeastSquaresProblemProto problem_proto; + { + string serialized_proto; + ReadFileToStringOrDie(filename, &serialized_proto); + CHECK(problem_proto.ParseFromString(serialized_proto)); + } + + LinearLeastSquaresProblem* problem = new LinearLeastSquaresProblem; + const SparseMatrixProto& A = problem_proto.a(); + + if (A.has_block_matrix()) { + problem->A.reset(new BlockSparseMatrix(A)); + } else if (A.has_triplet_matrix()) { + problem->A.reset(new TripletSparseMatrix(A)); + } else { + problem->A.reset(new CompressedRowSparseMatrix(A)); + } + + if (problem_proto.b_size() > 0) { + problem->b.reset(new double[problem_proto.b_size()]); + for (int i = 0; i < problem_proto.b_size(); ++i) { + problem->b[i] = problem_proto.b(i); + } + } + + if (problem_proto.d_size() > 0) { + problem->D.reset(new double[problem_proto.d_size()]); + for (int i = 0; i < problem_proto.d_size(); ++i) { + problem->D[i] = problem_proto.d(i); + } + } + + if (problem_proto.d_size() > 0) { + if (problem_proto.x_size() > 0) { + problem->x_D.reset(new double[problem_proto.x_size()]); + for (int i = 0; i < problem_proto.x_size(); ++i) { + problem->x_D[i] = problem_proto.x(i); + } + } + } else { + if (problem_proto.x_size() > 0) { + problem->x.reset(new double[problem_proto.x_size()]); + for (int i = 0; i < problem_proto.x_size(); ++i) { + problem->x[i] = problem_proto.x(i); + } + } + } + + problem->num_eliminate_blocks = 0; + if (problem_proto.has_num_eliminate_blocks()) { + problem->num_eliminate_blocks = problem_proto.num_eliminate_blocks(); + } + + return problem; +} +#else +LinearLeastSquaresProblem* CreateLinearLeastSquaresProblemFromFile( + const string& filename) { + LOG(FATAL) + << "Loading a least squares problem from disk requires " + << "Ceres to be built with Protocol Buffers support."; + return NULL; +} +#endif // CERES_DONT_HAVE_PROTOCOL_BUFFERS + +/* +A = [1 2] + [3 4] + [6 -10] + +b = [ 8 + 18 + -18] + +x = [2 + 3] + +D = [1 + 2] + +x_D = [1.78448275; + 2.82327586;] + */ +LinearLeastSquaresProblem* LinearLeastSquaresProblem0() { + LinearLeastSquaresProblem* problem = new LinearLeastSquaresProblem; + + TripletSparseMatrix* A = new TripletSparseMatrix(3, 2, 6); + problem->b.reset(new double[3]); + problem->D.reset(new double[2]); + + problem->x.reset(new double[2]); + problem->x_D.reset(new double[2]); + + int* Ai = A->mutable_rows(); + int* Aj = A->mutable_cols(); + double* Ax = A->mutable_values(); + + int counter = 0; + for (int i = 0; i < 3; ++i) { + for (int j = 0; j< 2; ++j) { + Ai[counter]=i; + Aj[counter]=j; + ++counter; + } + }; + + Ax[0] = 1.; + Ax[1] = 2.; + Ax[2] = 3.; + Ax[3] = 4.; + Ax[4] = 6; + Ax[5] = -10; + A->set_num_nonzeros(6); + problem->A.reset(A); + + problem->b[0] = 8; + problem->b[1] = 18; + problem->b[2] = -18; + + problem->x[0] = 2.0; + problem->x[1] = 3.0; + + problem->D[0] = 1; + problem->D[1] = 2; + + problem->x_D[0] = 1.78448275; + problem->x_D[1] = 2.82327586; + return problem; +} + + +/* + A = [1 0 | 2 0 0 + 3 0 | 0 4 0 + 0 5 | 0 0 6 + 0 7 | 8 0 0 + 0 9 | 1 0 0 + 0 0 | 1 1 1] + + b = [0 + 1 + 2 + 3 + 4 + 5] + + c = A'* b = [ 3 + 67 + 33 + 9 + 17] + + A'A = [10 0 2 12 0 + 0 155 65 0 30 + 2 65 70 1 1 + 12 0 1 17 1 + 0 30 1 1 37] + + S = [ 42.3419 -1.4000 -11.5806 + -1.4000 2.6000 1.0000 + 11.5806 1.0000 31.1935] + + r = [ 4.3032 + 5.4000 + 5.0323] + + S\r = [ 0.2102 + 2.1367 + 0.1388] + + A\b = [-2.3061 + 0.3172 + 0.2102 + 2.1367 + 0.1388] +*/ +// The following two functions create a TripletSparseMatrix and a +// BlockSparseMatrix version of this problem. + +// TripletSparseMatrix version. +LinearLeastSquaresProblem* LinearLeastSquaresProblem1() { + int num_rows = 6; + int num_cols = 5; + + LinearLeastSquaresProblem* problem = new LinearLeastSquaresProblem; + TripletSparseMatrix* A = new TripletSparseMatrix(num_rows, + num_cols, + num_rows * num_cols); + problem->b.reset(new double[num_rows]); + problem->D.reset(new double[num_cols]); + problem->num_eliminate_blocks = 2; + + int* rows = A->mutable_rows(); + int* cols = A->mutable_cols(); + double* values = A->mutable_values(); + + int nnz = 0; + + // Row 1 + { + rows[nnz] = 0; + cols[nnz] = 0; + values[nnz++] = 1; + + rows[nnz] = 0; + cols[nnz] = 2; + values[nnz++] = 2; + } + + // Row 2 + { + rows[nnz] = 1; + cols[nnz] = 0; + values[nnz++] = 3; + + rows[nnz] = 1; + cols[nnz] = 3; + values[nnz++] = 4; + } + + // Row 3 + { + rows[nnz] = 2; + cols[nnz] = 1; + values[nnz++] = 5; + + rows[nnz] = 2; + cols[nnz] = 4; + values[nnz++] = 6; + } + + // Row 4 + { + rows[nnz] = 3; + cols[nnz] = 1; + values[nnz++] = 7; + + rows[nnz] = 3; + cols[nnz] = 2; + values[nnz++] = 8; + } + + // Row 5 + { + rows[nnz] = 4; + cols[nnz] = 1; + values[nnz++] = 9; + + rows[nnz] = 4; + cols[nnz] = 2; + values[nnz++] = 1; + } + + // Row 6 + { + rows[nnz] = 5; + cols[nnz] = 2; + values[nnz++] = 1; + + rows[nnz] = 5; + cols[nnz] = 3; + values[nnz++] = 1; + + rows[nnz] = 5; + cols[nnz] = 4; + values[nnz++] = 1; + } + + A->set_num_nonzeros(nnz); + CHECK(A->IsValid()); + + problem->A.reset(A); + + for (int i = 0; i < num_cols; ++i) { + problem->D.get()[i] = 1; + } + + for (int i = 0; i < num_rows; ++i) { + problem->b.get()[i] = i; + } + + return problem; +} + +// BlockSparseMatrix version +LinearLeastSquaresProblem* LinearLeastSquaresProblem2() { + int num_rows = 6; + int num_cols = 5; + + LinearLeastSquaresProblem* problem = new LinearLeastSquaresProblem; + + problem->b.reset(new double[num_rows]); + problem->D.reset(new double[num_cols]); + problem->num_eliminate_blocks = 2; + + CompressedRowBlockStructure* bs = new CompressedRowBlockStructure; + scoped_array<double> values(new double[num_rows * num_cols]); + + for (int c = 0; c < num_cols; ++c) { + bs->cols.push_back(Block()); + bs->cols.back().size = 1; + bs->cols.back().position = c; + } + + int nnz = 0; + + // Row 1 + { + values[nnz++] = 1; + values[nnz++] = 2; + + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 0; + row.cells.push_back(Cell(0, 0)); + row.cells.push_back(Cell(2, 1)); + } + + // Row 2 + { + values[nnz++] = 3; + values[nnz++] = 4; + + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 1; + row.cells.push_back(Cell(0, 2)); + row.cells.push_back(Cell(3, 3)); + } + + // Row 3 + { + values[nnz++] = 5; + values[nnz++] = 6; + + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 2; + row.cells.push_back(Cell(1, 4)); + row.cells.push_back(Cell(4, 5)); + } + + // Row 4 + { + values[nnz++] = 7; + values[nnz++] = 8; + + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 3; + row.cells.push_back(Cell(1, 6)); + row.cells.push_back(Cell(2, 7)); + } + + // Row 5 + { + values[nnz++] = 9; + values[nnz++] = 1; + + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 4; + row.cells.push_back(Cell(1, 8)); + row.cells.push_back(Cell(2, 9)); + } + + // Row 6 + { + values[nnz++] = 1; + values[nnz++] = 1; + values[nnz++] = 1; + + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 5; + row.cells.push_back(Cell(2, 10)); + row.cells.push_back(Cell(3, 11)); + row.cells.push_back(Cell(4, 12)); + } + + BlockSparseMatrix* A = new BlockSparseMatrix(bs); + memcpy(A->mutable_values(), values.get(), nnz * sizeof(*A->values())); + + for (int i = 0; i < num_cols; ++i) { + problem->D.get()[i] = 1; + } + + for (int i = 0; i < num_rows; ++i) { + problem->b.get()[i] = i; + } + + problem->A.reset(A); + + return problem; +} + + +/* + A = [1 0 + 3 0 + 0 5 + 0 7 + 0 9 + 0 0] + + b = [0 + 1 + 2 + 3 + 4 + 5] +*/ +// BlockSparseMatrix version +LinearLeastSquaresProblem* LinearLeastSquaresProblem3() { + int num_rows = 5; + int num_cols = 2; + + LinearLeastSquaresProblem* problem = new LinearLeastSquaresProblem; + + problem->b.reset(new double[num_rows]); + problem->D.reset(new double[num_cols]); + problem->num_eliminate_blocks = 2; + + CompressedRowBlockStructure* bs = new CompressedRowBlockStructure; + scoped_array<double> values(new double[num_rows * num_cols]); + + for (int c = 0; c < num_cols; ++c) { + bs->cols.push_back(Block()); + bs->cols.back().size = 1; + bs->cols.back().position = c; + } + + int nnz = 0; + + // Row 1 + { + values[nnz++] = 1; + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 0; + row.cells.push_back(Cell(0, 0)); + } + + // Row 2 + { + values[nnz++] = 3; + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 1; + row.cells.push_back(Cell(0, 1)); + } + + // Row 3 + { + values[nnz++] = 5; + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 2; + row.cells.push_back(Cell(1, 2)); + } + + // Row 4 + { + values[nnz++] = 7; + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 3; + row.cells.push_back(Cell(1, 3)); + } + + // Row 5 + { + values[nnz++] = 9; + bs->rows.push_back(CompressedRow()); + CompressedRow& row = bs->rows.back(); + row.block.size = 1; + row.block.position = 4; + row.cells.push_back(Cell(1, 4)); + } + + BlockSparseMatrix* A = new BlockSparseMatrix(bs); + memcpy(A->mutable_values(), values.get(), nnz * sizeof(*A->values())); + + for (int i = 0; i < num_cols; ++i) { + problem->D.get()[i] = 1; + } + + for (int i = 0; i < num_rows; ++i) { + problem->b.get()[i] = i; + } + + problem->A.reset(A); + + return problem; +} + +bool DumpLinearLeastSquaresProblemToConsole(const string& directory, + int iteration, + const SparseMatrix* A, + const double* D, + const double* b, + const double* x, + int num_eliminate_blocks) { + CHECK_NOTNULL(A); + Matrix AA; + A->ToDenseMatrix(&AA); + LOG(INFO) << "A^T: \n" << AA.transpose(); + + if (D != NULL) { + LOG(INFO) << "A's appended diagonal:\n" + << ConstVectorRef(D, A->num_cols()); + } + + if (b != NULL) { + LOG(INFO) << "b: \n" << ConstVectorRef(b, A->num_rows()); + } + + if (x != NULL) { + LOG(INFO) << "x: \n" << ConstVectorRef(x, A->num_cols()); + } + return true; +}; + +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +bool DumpLinearLeastSquaresProblemToProtocolBuffer(const string& directory, + int iteration, + const SparseMatrix* A, + const double* D, + const double* b, + const double* x, + int num_eliminate_blocks) { + CHECK_NOTNULL(A); + LinearLeastSquaresProblemProto lsqp; + A->ToProto(lsqp.mutable_a()); + + if (D != NULL) { + for (int i = 0; i < A->num_cols(); ++i) { + lsqp.add_d(D[i]); + } + } + + if (b != NULL) { + for (int i = 0; i < A->num_rows(); ++i) { + lsqp.add_b(b[i]); + } + } + + if (x != NULL) { + for (int i = 0; i < A->num_cols(); ++i) { + lsqp.add_x(x[i]); + } + } + + lsqp.set_num_eliminate_blocks(num_eliminate_blocks); + string format_string = JoinPath(directory, + "lm_iteration_%03d.lsqp"); + string filename = + StringPrintf(format_string.c_str(), iteration); + LOG(INFO) << "Dumping least squares problem for iteration " << iteration + << " to disk. File: " << filename; + WriteStringToFileOrDie(lsqp.SerializeAsString(), filename); + return true; +} +#else +bool DumpLinearLeastSquaresProblemToProtocolBuffer(const string& directory, + int iteration, + const SparseMatrix* A, + const double* D, + const double* b, + const double* x, + int num_eliminate_blocks) { + LOG(ERROR) << "Dumping least squares problems is only " + << "supported when Ceres is compiled with " + << "protocol buffer support."; + return false; +} +#endif + +void WriteArrayToFileOrDie(const string& filename, + const double* x, + const int size) { + CHECK_NOTNULL(x); + VLOG(2) << "Writing array to: " << filename; + FILE* fptr = fopen(filename.c_str(), "w"); + CHECK_NOTNULL(fptr); + for (int i = 0; i < size; ++i) { + fprintf(fptr, "%17f\n", x[i]); + } + fclose(fptr); +} + +bool DumpLinearLeastSquaresProblemToTextFile(const string& directory, + int iteration, + const SparseMatrix* A, + const double* D, + const double* b, + const double* x, + int num_eliminate_blocks) { + CHECK_NOTNULL(A); + string format_string = JoinPath(directory, + "lm_iteration_%03d"); + string filename_prefix = + StringPrintf(format_string.c_str(), iteration); + + { + string filename = filename_prefix + "_A.txt"; + LOG(INFO) << "writing to: " << filename; + FILE* fptr = fopen(filename.c_str(), "w"); + CHECK_NOTNULL(fptr); + A->ToTextFile(fptr); + fclose(fptr); + } + + if (D != NULL) { + string filename = filename_prefix + "_D.txt"; + WriteArrayToFileOrDie(filename, D, A->num_cols()); + } + + if (b != NULL) { + string filename = filename_prefix + "_b.txt"; + WriteArrayToFileOrDie(filename, b, A->num_rows()); + } + + if (x != NULL) { + string filename = filename_prefix + "_x.txt"; + WriteArrayToFileOrDie(filename, x, A->num_cols()); + } + + return true; +} + +bool DumpLinearLeastSquaresProblem(const string& directory, + int iteration, + DumpFormatType dump_format_type, + const SparseMatrix* A, + const double* D, + const double* b, + const double* x, + int num_eliminate_blocks) { + switch (dump_format_type) { + case (CONSOLE): + return DumpLinearLeastSquaresProblemToConsole(directory, + iteration, + A, D, b, x, + num_eliminate_blocks); + case (PROTOBUF): + return DumpLinearLeastSquaresProblemToProtocolBuffer( + directory, + iteration, + A, D, b, x, + num_eliminate_blocks); + case (TEXTFILE): + return DumpLinearLeastSquaresProblemToTextFile(directory, + iteration, + A, D, b, x, + num_eliminate_blocks); + default: + LOG(FATAL) << "Unknown DumpFormatType " << dump_format_type; + }; + + return true; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/linear_least_squares_problems.h b/extern/libmv/third_party/ceres/internal/ceres/linear_least_squares_problems.h new file mode 100644 index 00000000000..553cc0d3db3 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/linear_least_squares_problems.h @@ -0,0 +1,87 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_LINEAR_LEAST_SQUARES_PROBLEMS_H_ +#define CERES_INTERNAL_LINEAR_LEAST_SQUARES_PROBLEMS_H_ + +#include <string> +#include <vector> +#include "ceres/sparse_matrix.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" + +namespace ceres { +namespace internal { + +// Structure defining a linear least squares problem and if possible +// ground truth solutions. To be used by various LinearSolver tests. +struct LinearLeastSquaresProblem { + LinearLeastSquaresProblem() + : A(NULL), b(NULL), D(NULL), num_eliminate_blocks(0), + x(NULL), x_D(NULL) { + } + + scoped_ptr<SparseMatrix> A; + scoped_array<double> b; + scoped_array<double> D; + // If using the schur eliminator then how many of the variable + // blocks are e_type blocks. + int num_eliminate_blocks; + + // Solution to min_x |Ax - b|^2 + scoped_array<double> x; + // Solution to min_x |Ax - b|^2 + |Dx|^2 + scoped_array<double> x_D; +}; + +// Factories for linear least squares problem. +LinearLeastSquaresProblem* CreateLinearLeastSquaresProblemFromId(int id); +LinearLeastSquaresProblem* CreateLinearLeastSquaresProblemFromFile( + const string& filename); + +LinearLeastSquaresProblem* LinearLeastSquaresProblem0(); +LinearLeastSquaresProblem* LinearLeastSquaresProblem1(); +LinearLeastSquaresProblem* LinearLeastSquaresProblem2(); +LinearLeastSquaresProblem* LinearLeastSquaresProblem3(); + +// Write the linear least squares problem to disk. The exact format +// depends on dump_format_type. +bool DumpLinearLeastSquaresProblem(const string& directory, + int iteration, + DumpFormatType dump_format_type, + const SparseMatrix* A, + const double* D, + const double* b, + const double* x, + int num_eliminate_blocks); +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_LINEAR_LEAST_SQUARES_PROBLEMS_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/linear_operator.cc b/extern/libmv/third_party/ceres/internal/ceres/linear_operator.cc new file mode 100644 index 00000000000..4b59fa13009 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/linear_operator.cc @@ -0,0 +1,40 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/linear_operator.h" + +namespace ceres { +namespace internal { + +LinearOperator::~LinearOperator() { +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/linear_operator.h b/extern/libmv/third_party/ceres/internal/ceres/linear_operator.h new file mode 100644 index 00000000000..d5c15cee6a9 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/linear_operator.h @@ -0,0 +1,59 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Base classes for access to an linear operator. + +#ifndef CERES_INTERNAL_LINEAR_OPERATOR_H_ +#define CERES_INTERNAL_LINEAR_OPERATOR_H_ + +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +// This is an abstract base class for linear operators. It supports +// access to size information and left and right multiply operators. +class LinearOperator { + public: + virtual ~LinearOperator(); + + // y = y + Ax; + virtual void RightMultiply(const double* x, double* y) const = 0; + // y = y + A'x; + virtual void LeftMultiply(const double* x, double* y) const = 0; + + virtual int num_rows() const = 0; + virtual int num_cols() const = 0; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_LINEAR_OPERATOR_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/linear_solver.cc b/extern/libmv/third_party/ceres/internal/ceres/linear_solver.cc new file mode 100644 index 00000000000..b2e3941eea1 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/linear_solver.cc @@ -0,0 +1,87 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/linear_solver.h" + +#include <glog/logging.h> +#include "ceres/cgnr_solver.h" +#include "ceres/dense_qr_solver.h" +#include "ceres/iterative_schur_complement_solver.h" +#include "ceres/schur_complement_solver.h" +#include "ceres/sparse_normal_cholesky_solver.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +LinearSolver::~LinearSolver() { +} + +LinearSolver* LinearSolver::Create(const LinearSolver::Options& options) { + switch (options.type) { + case CGNR: + return new CgnrSolver(options); + + case SPARSE_NORMAL_CHOLESKY: +#ifndef CERES_NO_SUITESPARSE + return new SparseNormalCholeskySolver(options); +#else + LOG(WARNING) << "SPARSE_NORMAL_CHOLESKY is not available. Please " + << "build Ceres with SuiteSparse. Returning NULL."; + return NULL; +#endif // CERES_NO_SUITESPARSE + + case SPARSE_SCHUR: +#ifndef CERES_NO_SUITESPARSE + return new SparseSchurComplementSolver(options); +#else + LOG(WARNING) << "SPARSE_SCHUR is not available. Please " + << "build Ceres with SuiteSparse. Returning NULL."; + return NULL; +#endif // CERES_NO_SUITESPARSE + + case DENSE_SCHUR: + return new DenseSchurComplementSolver(options); + + case ITERATIVE_SCHUR: + return new IterativeSchurComplementSolver(options); + + case DENSE_QR: + return new DenseQRSolver(options); + + default: + LOG(FATAL) << "Unknown linear solver type :" + << options.type; + return NULL; // MSVC doesn't understand that LOG(FATAL) never returns. + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/linear_solver.h b/extern/libmv/third_party/ceres/internal/ceres/linear_solver.h new file mode 100644 index 00000000000..5860ecc8a77 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/linear_solver.h @@ -0,0 +1,291 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Abstract interface for objects solving linear systems of various +// kinds. + +#ifndef CERES_INTERNAL_LINEAR_SOLVER_H_ +#define CERES_INTERNAL_LINEAR_SOLVER_H_ + +#include <cstddef> + +#include <glog/logging.h> +#include "ceres/block_sparse_matrix.h" +#include "ceres/casts.h" +#include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/dense_sparse_matrix.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +class LinearOperator; + +// Abstract base class for objects that implement algorithms for +// solving linear systems +// +// Ax = b +// +// It is expected that a single instance of a LinearSolver object +// maybe used multiple times for solving different linear +// systems. This allows them to cache and reuse information across +// solves if for example the sparsity of the linear system remains +// constant. +// +// Subclasses of LinearSolver use two structs to configure themselves. +// The Options struct configures the LinearSolver object for its +// lifetime. The PerSolveOptions struct is used to specify options for +// a particular Solve call. +class LinearSolver { + public: + struct Options { + Options() + : type(SPARSE_NORMAL_CHOLESKY), + preconditioner_type(JACOBI), + min_num_iterations(1), + max_num_iterations(1), + num_threads(1), + constant_sparsity(false), + num_eliminate_blocks(0), + residual_reset_period(10), + row_block_size(Dynamic), + e_block_size(Dynamic), + f_block_size(Dynamic) { + } + + LinearSolverType type; + + PreconditionerType preconditioner_type; + + // Number of internal iterations that the solver uses. This + // parameter only makes sense for iterative solvers like CG. + int min_num_iterations; + int max_num_iterations; + + // If possible, how many threads can the solver use. + int num_threads; + + // If possible cache and reuse the symbolic factorization across + // multiple calls. + bool constant_sparsity; + + // Eliminate 0 to num_eliminate_blocks - 1 from the Normal + // equations to form a schur complement. Only used by the Schur + // complement based solver. The most common use for this parameter + // is in the case of structure from motion problems where we have + // camera blocks and point blocks. Then setting the + // num_eliminate_blocks to the number of points allows the solver + // to use the Schur complement trick. For more details see the + // description of this parameter in solver.h. + int num_eliminate_blocks; + + // Iterative solvers, e.g. Preconditioned Conjugate Gradients + // maintain a cheap estimate of the residual which may become + // inaccurate over time. Thus for non-zero values of this + // parameter, the solver can be told to recalculate the value of + // the residual using a |b - Ax| evaluation. + int residual_reset_period; + + // If the block sizes in a BlockSparseMatrix are fixed, then in + // some cases the Schur complement based solvers can detect and + // specialize on them. + // + // It is expected that these parameters are set programmatically + // rather than manually. + // + // Please see explicit_schur_complement_solver_impl.h for more + // details. + int row_block_size; + int e_block_size; + int f_block_size; + }; + + // Options for the Solve method. + struct PerSolveOptions { + PerSolveOptions() + : D(NULL), + preconditioner(NULL), + r_tolerance(0.0), + q_tolerance(0.0) { + } + + // This option only makes sense for unsymmetric linear solvers + // that can solve rectangular linear systems. + // + // Given a matrix A, an optional diagonal matrix D as a vector, + // and a vector b, the linear solver will solve for + // + // | A | x = | b | + // | D | | 0 | + // + // If D is null, then it is treated as zero, and the solver returns + // the solution to + // + // A x = b + // + // In either case, x is the vector that solves the following + // optimization problem. + // + // arg min_x ||Ax - b||^2 + ||Dx||^2 + // + // Here A is a matrix of size m x n, with full column rank. If A + // does not have full column rank, the results returned by the + // solver cannot be relied on. D, if it is not null is an array of + // size n. b is an array of size m and x is an array of size n. + double * D; + + // This option only makes sense for iterative solvers. + // + // In general the performance of an iterative linear solver + // depends on the condition number of the matrix A. For example + // the convergence rate of the conjugate gradients algorithm + // is proportional to the square root of the condition number. + // + // One particularly useful technique for improving the + // conditioning of a linear system is to precondition it. In its + // simplest form a preconditioner is a matrix M such that instead + // of solving Ax = b, we solve the linear system AM^{-1} y = b + // instead, where M is such that the condition number k(AM^{-1}) + // is smaller than the conditioner k(A). Given the solution to + // this system, x = M^{-1} y. The iterative solver takes care of + // the mechanics of solving the preconditioned system and + // returning the corrected solution x. The user only needs to + // supply a linear operator. + // + // A null preconditioner is equivalent to an identity matrix being + // used a preconditioner. + LinearOperator* preconditioner; + + + // The following tolerance related options only makes sense for + // iterative solvers. Direct solvers ignore them. + + // Solver terminates when + // + // |Ax - b| <= r_tolerance * |b|. + // + // This is the most commonly used termination criterion for + // iterative solvers. + double r_tolerance; + + // For PSD matrices A, let + // + // Q(x) = x'Ax - 2b'x + // + // be the cost of the quadratic function defined by A and b. Then, + // the solver terminates at iteration i if + // + // i * (Q(x_i) - Q(x_i-1)) / Q(x_i) < q_tolerance. + // + // This termination criterion is more useful when using CG to + // solve the Newton step. This particular convergence test comes + // from Stephen Nash's work on truncated Newton + // methods. References: + // + // 1. Stephen G. Nash & Ariela Sofer, Assessing A Search + // Direction Within A Truncated Newton Method, Operation + // Research Letters 9(1990) 219-221. + // + // 2. Stephen G. Nash, A Survey of Truncated Newton Methods, + // Journal of Computational and Applied Mathematics, + // 124(1-2), 45-59, 2000. + // + double q_tolerance; + }; + + // Summary of a call to the Solve method. We should move away from + // the true/false method for determining solver success. We should + // let the summary object do the talking. + struct Summary { + Summary() + : residual_norm(0.0), + num_iterations(-1), + termination_type(FAILURE) { + } + + double residual_norm; + int num_iterations; + LinearSolverTerminationType termination_type; + }; + + virtual ~LinearSolver(); + + // Solve Ax = b. + virtual Summary Solve(LinearOperator* A, + const double* b, + const PerSolveOptions& per_solve_options, + double* x) = 0; + + static LinearSolver* Create(const Options& options); +}; + +// This templated subclass of LinearSolver serves as a base class for +// other linear solvers that depend on the particular matrix layout of +// the underlying linear operator. For example some linear solvers +// need low level access to the TripletSparseMatrix implementing the +// LinearOperator interface. This class hides those implementation +// details behind a private virtual method, and has the Solve method +// perform the necessary upcasting. +template <typename MatrixType> +class TypedLinearSolver : public LinearSolver { + public: + virtual ~TypedLinearSolver() {} + virtual LinearSolver::Summary Solve( + LinearOperator* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x) { + CHECK_NOTNULL(A); + CHECK_NOTNULL(b); + CHECK_NOTNULL(x); + return SolveImpl(down_cast<MatrixType*>(A), b, per_solve_options, x); + } + + private: + virtual LinearSolver::Summary SolveImpl( + MatrixType* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x) = 0; +}; + +// Linear solvers that depend on acccess to the low level structure of +// a SparseMatrix. +typedef TypedLinearSolver<BlockSparseMatrix> BlockSparseMatrixSolver; // NOLINT +typedef TypedLinearSolver<BlockSparseMatrixBase> BlockSparseMatrixBaseSolver; // NOLINT +typedef TypedLinearSolver<CompressedRowSparseMatrix> CompressedRowSparseMatrixSolver; // NOLINT +typedef TypedLinearSolver<DenseSparseMatrix> DenseSparseMatrixSolver; // NOLINT +typedef TypedLinearSolver<TripletSparseMatrix> TripletSparseMatrixSolver; // NOLINT + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_LINEAR_SOLVER_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/local_parameterization.cc b/extern/libmv/third_party/ceres/internal/ceres/local_parameterization.cc new file mode 100644 index 00000000000..eeae74e3f95 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/local_parameterization.cc @@ -0,0 +1,140 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include <glog/logging.h> +#include "ceres/internal/eigen.h" +#include "ceres/local_parameterization.h" +#include "ceres/rotation.h" + +namespace ceres { + +IdentityParameterization::IdentityParameterization(const int size) + : size_(size) { + CHECK_GT(size, 0); +} + +bool IdentityParameterization::Plus(const double* x, + const double* delta, + double* x_plus_delta) const { + VectorRef(x_plus_delta, size_) = + ConstVectorRef(x, size_) + ConstVectorRef(delta, size_); + return true; +} + +bool IdentityParameterization::ComputeJacobian(const double* x, + double* jacobian) const { + MatrixRef(jacobian, size_, size_) = Matrix::Identity(size_, size_); + return true; +} + +SubsetParameterization::SubsetParameterization( + int size, + const vector<int>& constant_parameters) + : local_size_(size - constant_parameters.size()), + constancy_mask_(size, 0) { + CHECK_GT(constant_parameters.size(), 0) + << "The set of constant parameters should contain at least " + << "one element. If you do not wish to hold any parameters " + << "constant, then do not use a SubsetParameterization"; + + vector<int> constant = constant_parameters; + sort(constant.begin(), constant.end()); + CHECK(unique(constant.begin(), constant.end()) == constant.end()) + << "The set of constant parameters cannot contain duplicates"; + CHECK_LT(constant_parameters.size(), size) + << "Number of parameters held constant should be less " + << "than the size of the parameter block. If you wish " + << "to hold the entire parameter block constant, then a " + << "efficient way is to directly mark it as constant " + << "instead of using a LocalParameterization to do so."; + CHECK_GE(*min_element(constant.begin(), constant.end()), 0); + CHECK_LT(*max_element(constant.begin(), constant.end()), size); + + for (int i = 0; i < constant_parameters.size(); ++i) { + constancy_mask_[constant_parameters[i]] = 1; + } +} + +bool SubsetParameterization::Plus(const double* x, + const double* delta, + double* x_plus_delta) const { + for (int i = 0, j = 0; i < constancy_mask_.size(); ++i) { + if (constancy_mask_[i]) { + x_plus_delta[i] = x[i]; + } else { + x_plus_delta[i] = x[i] + delta[j++]; + } + } + return true; +} + +bool SubsetParameterization::ComputeJacobian(const double* x, + double* jacobian) const { + MatrixRef m(jacobian, constancy_mask_.size(), local_size_); + m.setZero(); + for (int i = 0, j = 0; i < constancy_mask_.size(); ++i) { + if (!constancy_mask_[i]) { + m(i, j++) = 1.0; + } + } + return true; +} + +bool QuaternionParameterization::Plus(const double* x, + const double* delta, + double* x_plus_delta) const { + const double norm_delta = + sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]); + if (norm_delta > 0.0) { + const double sin_delta_by_delta = (sin(norm_delta) / norm_delta); + double q_delta[4]; + q_delta[0] = cos(norm_delta); + q_delta[1] = sin_delta_by_delta * delta[0]; + q_delta[2] = sin_delta_by_delta * delta[1]; + q_delta[3] = sin_delta_by_delta * delta[2]; + QuaternionProduct(q_delta, x, x_plus_delta); + } else { + for (int i = 0; i < 4; ++i) { + x_plus_delta[i] = x[i]; + } + } + return true; +} + +bool QuaternionParameterization::ComputeJacobian(const double* x, + double* jacobian) const { + jacobian[0] = -x[1]; jacobian[1] = -x[2]; jacobian[2] = -x[3]; // NOLINT + jacobian[3] = x[0]; jacobian[4] = x[3]; jacobian[5] = -x[2]; // NOLINT + jacobian[6] = -x[3]; jacobian[7] = x[0]; jacobian[8] = x[1]; // NOLINT + jacobian[9] = x[2]; jacobian[10] = -x[1]; jacobian[11] = x[0]; // NOLINT + return true; +} + +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/loss_function.cc b/extern/libmv/third_party/ceres/internal/ceres/loss_function.cc new file mode 100644 index 00000000000..00b2b184729 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/loss_function.cc @@ -0,0 +1,93 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Purpose: See .h file. + +#include "ceres/loss_function.h" + +#include <cmath> +#include <cstddef> + +namespace ceres { + +void TrivialLoss::Evaluate(double s, double rho[3]) const { + rho[0] = s; + rho[1] = 1; + rho[2] = 0; +} + +void HuberLoss::Evaluate(double s, double rho[3]) const { + if (s > b_) { + // Outlier region. + // 'r' is always positive. + const double r = sqrt(s); + rho[0] = 2 * a_ * r - b_; + rho[1] = a_ / r; + rho[2] = - rho[1] / (2 * s); + } else { + // Inlier region. + rho[0] = s; + rho[1] = 1; + rho[2] = 0; + } +} + +void SoftLOneLoss::Evaluate(double s, double rho[3]) const { + const double sum = 1 + s * c_; + const double tmp = sqrt(sum); + // 'sum' and 'tmp' are always positive, assuming that 's' is. + rho[0] = 2 * b_ * (tmp - 1); + rho[1] = 1 / tmp; + rho[2] = - (c_ * rho[1]) / (2 * sum); +} + +void CauchyLoss::Evaluate(double s, double rho[3]) const { + const double sum = 1 + s * c_; + const double inv = 1 / sum; + // 'sum' and 'inv' are always positive, assuming that 's' is. + rho[0] = b_ * log(sum); + rho[1] = inv; + rho[2] = - c_ * (inv * inv); +} + +void ScaledLoss::Evaluate(double s, double rho[3]) const { + if (rho_.get() == NULL) { + rho[0] = a_ * s; + rho[1] = a_; + rho[2] = 0.0; + } else { + rho_->Evaluate(s, rho); + rho[0] *= a_; + rho[1] *= a_; + rho[2] *= a_; + } +} + +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/map_util.h b/extern/libmv/third_party/ceres/internal/ceres/map_util.h new file mode 100644 index 00000000000..ddf1252f674 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/map_util.h @@ -0,0 +1,129 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// Originally by Anton Carver + +#ifndef CERES_INTERNAL_MAP_UTIL_H_ +#define CERES_INTERNAL_MAP_UTIL_H_ + +#include <utility> +#include "ceres/internal/port.h" + +namespace ceres { + +// Perform a lookup in a map or hash_map, assuming that the key exists. +// Crash if it does not. +// +// This is intended as a replacement for operator[] as an rvalue (for reading) +// when the key is guaranteed to exist. +// +// operator[] is discouraged for several reasons: +// * It has a side-effect of inserting missing keys +// * It is not thread-safe (even when it is not inserting, it can still +// choose to resize the underlying storage) +// * It invalidates iterators (when it chooses to resize) +// * It default constructs a value object even if it doesn't need to +// +// This version assumes the key is printable, and includes it in the fatal log +// message. +template <class Collection> +const typename Collection::value_type::second_type& +FindOrDie(const Collection& collection, + const typename Collection::value_type::first_type& key) { + typename Collection::const_iterator it = collection.find(key); + CHECK(it != collection.end()) << "Map key not found: " << key; + return it->second; +} + +// Perform a lookup in a map or hash_map. +// If the key is present in the map then the value associated with that +// key is returned, otherwise the value passed as a default is returned. +template <class Collection> +const typename Collection::value_type::second_type& +FindWithDefault(const Collection& collection, + const typename Collection::value_type::first_type& key, + const typename Collection::value_type::second_type& value) { + typename Collection::const_iterator it = collection.find(key); + if (it == collection.end()) { + return value; + } + return it->second; +} + +// Insert a new key and value into a map or hash_map. +// If the key is not present in the map the key and value are +// inserted, otherwise nothing happens. True indicates that an insert +// took place, false indicates the key was already present. +template <class Collection> +bool InsertIfNotPresent( + Collection * const collection, + const typename Collection::value_type::first_type& key, + const typename Collection::value_type::second_type& value) { + pair<typename Collection::iterator, bool> ret = + collection->insert(typename Collection::value_type(key, value)); + return ret.second; +} + +// Perform a lookup in a map or hash_map. +// Same as above but the returned pointer is not const and can be used to change +// the stored value. +template <class Collection> +typename Collection::value_type::second_type* +FindOrNull(Collection& collection, // NOLINT + const typename Collection::value_type::first_type& key) { + typename Collection::iterator it = collection.find(key); + if (it == collection.end()) { + return 0; + } + return &it->second; +} + +// Test to see if a set, map, hash_set or hash_map contains a particular key. +// Returns true if the key is in the collection. +template <class Collection, class Key> +bool ContainsKey(const Collection& collection, const Key& key) { + typename Collection::const_iterator it = collection.find(key); + return it != collection.end(); +} + +// Inserts a new key/value into a map or hash_map. +// Dies if the key is already present. +template<class Collection> +void InsertOrDie(Collection* const collection, + const typename Collection::value_type::first_type& key, + const typename Collection::value_type::second_type& data) { + typedef typename Collection::value_type value_type; + CHECK(collection->insert(value_type(key, data)).second) + << "duplicate key: " << key; +} + +} // namespace ceres + +#endif // CERES_INTERNAL_MAP_UTIL_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/matrix_proto.h b/extern/libmv/third_party/ceres/internal/ceres/matrix_proto.h new file mode 100644 index 00000000000..b8a3a1a6de6 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/matrix_proto.h @@ -0,0 +1,40 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// A portability header to make optional protocol buffer support less intrusive. + +#ifndef CERES_INTERNAL_MATRIX_PROTO_H_ +#define CERES_INTERNAL_MATRIX_PROTO_H_ + +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +#include "ceres/matrix.pb.h" +#endif + +#endif // CERES_INTERNAL_MATRIX_PROTO_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/minimizer.h b/extern/libmv/third_party/ceres/internal/ceres/minimizer.h new file mode 100644 index 00000000000..77cb00cb6b4 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/minimizer.h @@ -0,0 +1,104 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_MINIMIZER_H_ +#define CERES_INTERNAL_MINIMIZER_H_ + +#include <vector> +#include "ceres/solver.h" +#include "ceres/iteration_callback.h" + +namespace ceres { +namespace internal { + +class Evaluator; +class LinearSolver; + +// Interface for non-linear least squares solvers. +class Minimizer { + public: + // Options struct to control the behaviour of the Minimizer. Please + // see solver.h for detailed information about the meaning and + // default values of each of these parameters. + struct Options { + explicit Options(const Solver::Options& options) { + max_num_iterations = options.max_num_iterations; + max_solver_time_sec = options.max_solver_time_sec; + gradient_tolerance = options.gradient_tolerance; + parameter_tolerance = options.parameter_tolerance; + function_tolerance = options.function_tolerance; + min_relative_decrease = options.min_relative_decrease; + eta = options.eta; + tau = options.tau; + jacobi_scaling = options.jacobi_scaling; + crash_and_dump_lsqp_on_failure = options.crash_and_dump_lsqp_on_failure; + lsqp_dump_directory = options.lsqp_dump_directory; + lsqp_iterations_to_dump = options.lsqp_iterations_to_dump; + lsqp_dump_format_type = options.lsqp_dump_format_type; + num_eliminate_blocks = options.num_eliminate_blocks; + logging_type = options.logging_type; + } + + int max_num_iterations; + int max_solver_time_sec; + double gradient_tolerance; + double parameter_tolerance; + double function_tolerance; + double min_relative_decrease; + double eta; + double tau; + bool jacobi_scaling; + bool crash_and_dump_lsqp_on_failure; + vector<int> lsqp_iterations_to_dump; + DumpFormatType lsqp_dump_format_type; + string lsqp_dump_directory; + int num_eliminate_blocks; + LoggingType logging_type; + + // List of callbacks that are executed by the Minimizer at the end + // of each iteration. + // + // Client owns these pointers. + vector<IterationCallback*> callbacks; + }; + + virtual ~Minimizer() {} + virtual void Minimize(const Options& options, + Evaluator* evaluator, + LinearSolver* linear_solver, + const double* initial_parameters, + double* final_parameters, + Solver::Summary* summary) = 0; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_MINIMIZER_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/mutex.h b/extern/libmv/third_party/ceres/internal/ceres/mutex.h new file mode 100644 index 00000000000..6514b107041 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/mutex.h @@ -0,0 +1,312 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: Craig Silverstein. +// +// A simple mutex wrapper, supporting locks and read-write locks. +// You should assume the locks are *not* re-entrant. +// +// This class is meant to be internal-only and should be wrapped by an +// internal namespace. Before you use this module, please give the +// name of your internal namespace for this module. Or, if you want +// to expose it, you'll want to move it to the Google namespace. We +// cannot put this class in global namespace because there can be some +// problems when we have multiple versions of Mutex in each shared object. +// +// NOTE: by default, we have #ifdef'ed out the TryLock() method. +// This is for two reasons: +// 1) TryLock() under Windows is a bit annoying (it requires a +// #define to be defined very early). +// 2) TryLock() is broken for NO_THREADS mode, at least in NDEBUG +// mode. +// If you need TryLock(), and either these two caveats are not a +// problem for you, or you're willing to work around them, then +// feel free to #define GMUTEX_TRYLOCK, or to remove the #ifdefs +// in the code below. +// +// CYGWIN NOTE: Cygwin support for rwlock seems to be buggy: +// http://www.cygwin.com/ml/cygwin/2008-12/msg00017.html +// Because of that, we might as well use windows locks for +// cygwin. They seem to be more reliable than the cygwin pthreads layer. +// +// TRICKY IMPLEMENTATION NOTE: +// This class is designed to be safe to use during +// dynamic-initialization -- that is, by global constructors that are +// run before main() starts. The issue in this case is that +// dynamic-initialization happens in an unpredictable order, and it +// could be that someone else's dynamic initializer could call a +// function that tries to acquire this mutex -- but that all happens +// before this mutex's constructor has run. (This can happen even if +// the mutex and the function that uses the mutex are in the same .cc +// file.) Basically, because Mutex does non-trivial work in its +// constructor, it's not, in the naive implementation, safe to use +// before dynamic initialization has run on it. +// +// The solution used here is to pair the actual mutex primitive with a +// bool that is set to true when the mutex is dynamically initialized. +// (Before that it's false.) Then we modify all mutex routines to +// look at the bool, and not try to lock/unlock until the bool makes +// it to true (which happens after the Mutex constructor has run.) +// +// This works because before main() starts -- particularly, during +// dynamic initialization -- there are no threads, so a) it's ok that +// the mutex operations are a no-op, since we don't need locking then +// anyway; and b) we can be quite confident our bool won't change +// state between a call to Lock() and a call to Unlock() (that would +// require a global constructor in one translation unit to call Lock() +// and another global constructor in another translation unit to call +// Unlock() later, which is pretty perverse). +// +// That said, it's tricky, and can conceivably fail; it's safest to +// avoid trying to acquire a mutex in a global constructor, if you +// can. One way it can fail is that a really smart compiler might +// initialize the bool to true at static-initialization time (too +// early) rather than at dynamic-initialization time. To discourage +// that, we set is_safe_ to true in code (not the constructor +// colon-initializer) and set it to true via a function that always +// evaluates to true, but that the compiler can't know always +// evaluates to true. This should be good enough. + +#ifndef CERES_INTERNAL_MUTEX_H_ +#define CERES_INTERNAL_MUTEX_H_ + +#if defined(NO_THREADS) + typedef int MutexType; // to keep a lock-count +#elif defined(_WIN32) || defined(__CYGWIN32__) || defined(__CYGWIN64__) +# define WIN32_LEAN_AND_MEAN // We only need minimal includes +# ifdef GMUTEX_TRYLOCK + // We need Windows NT or later for TryEnterCriticalSection(). If you + // don't need that functionality, you can remove these _WIN32_WINNT + // lines, and change TryLock() to assert(0) or something. +# ifndef _WIN32_WINNT +# define _WIN32_WINNT 0x0400 +# endif +# endif +// To avoid macro definition of ERROR. +# define NOGDI +// To avoid macro definition of min/max. +# define NOMINMAX +# include <windows.h> + typedef CRITICAL_SECTION MutexType; +#elif defined(CERES_HAVE_PTHREAD) && defined(CERES_HAVE_RWLOCK) + // Needed for pthread_rwlock_*. If it causes problems, you could take it + // out, but then you'd have to unset CERES_HAVE_RWLOCK (at least on linux -- + // it *does* cause problems for FreeBSD, or MacOSX, but isn't needed for + // locking there.) +# if defined(__linux__) && !defined(_XOPEN_SOURCE) +# define _XOPEN_SOURCE 500 // may be needed to get the rwlock calls +# endif +# include <pthread.h> + typedef pthread_rwlock_t MutexType; +#elif defined(CERES_HAVE_PTHREAD) +# include <pthread.h> + typedef pthread_mutex_t MutexType; +#else +# error Need to implement mutex.h for your architecture, or #define NO_THREADS +#endif + +// We need to include these header files after defining _XOPEN_SOURCE +// as they may define the _XOPEN_SOURCE macro. +#include <assert.h> +#include <stdlib.h> // for abort() + +namespace ceres { +namespace internal { + +class Mutex { + public: + // Create a Mutex that is not held by anybody. This constructor is + // typically used for Mutexes allocated on the heap or the stack. + // See below for a recommendation for constructing global Mutex + // objects. + inline Mutex(); + + // Destructor + inline ~Mutex(); + + inline void Lock(); // Block if needed until free then acquire exclusively + inline void Unlock(); // Release a lock acquired via Lock() +#ifdef GMUTEX_TRYLOCK + inline bool TryLock(); // If free, Lock() and return true, else return false +#endif + // Note that on systems that don't support read-write locks, these may + // be implemented as synonyms to Lock() and Unlock(). So you can use + // these for efficiency, but don't use them anyplace where being able + // to do shared reads is necessary to avoid deadlock. + inline void ReaderLock(); // Block until free or shared then acquire a share + inline void ReaderUnlock(); // Release a read share of this Mutex + inline void WriterLock() { Lock(); } // Acquire an exclusive lock + inline void WriterUnlock() { Unlock(); } // Release a lock from WriterLock() + + // TODO(hamaji): Do nothing, implement correctly. + inline void AssertHeld() {} + + private: + MutexType mutex_; + // We want to make sure that the compiler sets is_safe_ to true only + // when we tell it to, and never makes assumptions is_safe_ is + // always true. volatile is the most reliable way to do that. + volatile bool is_safe_; + + inline void SetIsSafe() { is_safe_ = true; } + + // Catch the error of writing Mutex when intending MutexLock. + Mutex(Mutex* /*ignored*/) {} + // Disallow "evil" constructors + Mutex(const Mutex&); + void operator=(const Mutex&); +}; + +// Now the implementation of Mutex for various systems +#if defined(NO_THREADS) + +// When we don't have threads, we can be either reading or writing, +// but not both. We can have lots of readers at once (in no-threads +// mode, that's most likely to happen in recursive function calls), +// but only one writer. We represent this by having mutex_ be -1 when +// writing and a number > 0 when reading (and 0 when no lock is held). +// +// In debug mode, we assert these invariants, while in non-debug mode +// we do nothing, for efficiency. That's why everything is in an +// assert. + +Mutex::Mutex() : mutex_(0) { } +Mutex::~Mutex() { assert(mutex_ == 0); } +void Mutex::Lock() { assert(--mutex_ == -1); } +void Mutex::Unlock() { assert(mutex_++ == -1); } +#ifdef GMUTEX_TRYLOCK +bool Mutex::TryLock() { if (mutex_) return false; Lock(); return true; } +#endif +void Mutex::ReaderLock() { assert(++mutex_ > 0); } +void Mutex::ReaderUnlock() { assert(mutex_-- > 0); } + +#elif defined(_WIN32) || defined(__CYGWIN32__) || defined(__CYGWIN64__) + +Mutex::Mutex() { InitializeCriticalSection(&mutex_); SetIsSafe(); } +Mutex::~Mutex() { DeleteCriticalSection(&mutex_); } +void Mutex::Lock() { if (is_safe_) EnterCriticalSection(&mutex_); } +void Mutex::Unlock() { if (is_safe_) LeaveCriticalSection(&mutex_); } +#ifdef GMUTEX_TRYLOCK +bool Mutex::TryLock() { return is_safe_ ? + TryEnterCriticalSection(&mutex_) != 0 : true; } +#endif +void Mutex::ReaderLock() { Lock(); } // we don't have read-write locks +void Mutex::ReaderUnlock() { Unlock(); } + +#elif defined(CERES_HAVE_PTHREAD) && defined(CERES_HAVE_RWLOCK) + +#define SAFE_PTHREAD(fncall) do { /* run fncall if is_safe_ is true */ \ + if (is_safe_ && fncall(&mutex_) != 0) abort(); \ +} while (0) + +Mutex::Mutex() { + SetIsSafe(); + if (is_safe_ && pthread_rwlock_init(&mutex_, NULL) != 0) abort(); +} +Mutex::~Mutex() { SAFE_PTHREAD(pthread_rwlock_destroy); } +void Mutex::Lock() { SAFE_PTHREAD(pthread_rwlock_wrlock); } +void Mutex::Unlock() { SAFE_PTHREAD(pthread_rwlock_unlock); } +#ifdef GMUTEX_TRYLOCK +bool Mutex::TryLock() { return is_safe_ ? + pthread_rwlock_trywrlock(&mutex_) == 0 : + true; } +#endif +void Mutex::ReaderLock() { SAFE_PTHREAD(pthread_rwlock_rdlock); } +void Mutex::ReaderUnlock() { SAFE_PTHREAD(pthread_rwlock_unlock); } +#undef SAFE_PTHREAD + +#elif defined(CERES_HAVE_PTHREAD) + +#define SAFE_PTHREAD(fncall) do { /* run fncall if is_safe_ is true */ \ + if (is_safe_ && fncall(&mutex_) != 0) abort(); \ +} while (0) + +Mutex::Mutex() { + SetIsSafe(); + if (is_safe_ && pthread_mutex_init(&mutex_, NULL) != 0) abort(); +} +Mutex::~Mutex() { SAFE_PTHREAD(pthread_mutex_destroy); } +void Mutex::Lock() { SAFE_PTHREAD(pthread_mutex_lock); } +void Mutex::Unlock() { SAFE_PTHREAD(pthread_mutex_unlock); } +#ifdef GMUTEX_TRYLOCK +bool Mutex::TryLock() { return is_safe_ ? + pthread_mutex_trylock(&mutex_) == 0 : true; } +#endif +void Mutex::ReaderLock() { Lock(); } +void Mutex::ReaderUnlock() { Unlock(); } +#undef SAFE_PTHREAD + +#endif + +// -------------------------------------------------------------------------- +// Some helper classes + +// MutexLock(mu) acquires mu when constructed and releases it when destroyed. +class MutexLock { + public: + explicit MutexLock(Mutex *mu) : mu_(mu) { mu_->Lock(); } + ~MutexLock() { mu_->Unlock(); } + private: + Mutex * const mu_; + // Disallow "evil" constructors + MutexLock(const MutexLock&); + void operator=(const MutexLock&); +}; + +// ReaderMutexLock and WriterMutexLock do the same, for rwlocks +class ReaderMutexLock { + public: + explicit ReaderMutexLock(Mutex *mu) : mu_(mu) { mu_->ReaderLock(); } + ~ReaderMutexLock() { mu_->ReaderUnlock(); } + private: + Mutex * const mu_; + // Disallow "evil" constructors + ReaderMutexLock(const ReaderMutexLock&); + void operator=(const ReaderMutexLock&); +}; + +class WriterMutexLock { + public: + explicit WriterMutexLock(Mutex *mu) : mu_(mu) { mu_->WriterLock(); } + ~WriterMutexLock() { mu_->WriterUnlock(); } + private: + Mutex * const mu_; + // Disallow "evil" constructors + WriterMutexLock(const WriterMutexLock&); + void operator=(const WriterMutexLock&); +}; + +// Catch bug where variable name is omitted, e.g. MutexLock (&mu); +#define MutexLock(x) COMPILE_ASSERT(0, mutex_lock_decl_missing_var_name) +#define ReaderMutexLock(x) COMPILE_ASSERT(0, rmutex_lock_decl_missing_var_name) +#define WriterMutexLock(x) COMPILE_ASSERT(0, wmutex_lock_decl_missing_var_name) + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_MUTEX_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/normal_prior.cc b/extern/libmv/third_party/ceres/internal/ceres/normal_prior.cc new file mode 100644 index 00000000000..f30bbc8b46b --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/normal_prior.cc @@ -0,0 +1,67 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/normal_prior.h" + +#include <cstddef> +#include <vector> + +#include <glog/logging.h> +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { + +NormalPrior::NormalPrior(const Matrix& A, const Vector& b) + : A_(A), b_(b) { + CHECK_GT(b_.rows(), 0); + CHECK_GT(A_.rows(), 0); + CHECK_EQ(b_.rows(), A.cols()); + set_num_residuals(A_.rows()); + mutable_parameter_block_sizes()->push_back(b_.rows()); +} + +bool NormalPrior::Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const { + ConstVectorRef p(parameters[0], parameter_block_sizes()[0]); + VectorRef r(residuals, num_residuals()); + // The following line should read + // r = A_ * (p - b_); + // The extra eval is to get around a bug in the eigen library. + r = A_ * (p - b_).eval(); + if ((jacobians != NULL) && (jacobians[0] != NULL)) { + MatrixRef(jacobians[0], num_residuals(), parameter_block_sizes()[0]) = A_; + } + return true; +} + +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/parameter_block.h b/extern/libmv/third_party/ceres/internal/ceres/parameter_block.h new file mode 100644 index 00000000000..4bac1a85828 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/parameter_block.h @@ -0,0 +1,256 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#ifndef CERES_INTERNAL_PARAMETER_BLOCK_H_ +#define CERES_INTERNAL_PARAMETER_BLOCK_H_ + +#include <cstdlib> +#include "ceres/integral_types.h" +#include <glog/logging.h> +#include "ceres/internal/eigen.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/local_parameterization.h" +#include "ceres/residual_block_utils.h" + +namespace ceres { +namespace internal { + +class ProblemImpl; + +// The parameter block encodes the location of the user's original value, and +// also the "current state" of the parameter. The evaluator uses whatever is in +// the current state of the parameter when evaluating. This is inlined since the +// methods are performance sensitive. +// +// The class is not thread-safe, unless only const methods are called. The +// parameter block may also hold a pointer to a local parameterization; the +// parameter block does not take ownership of this pointer, so the user is +// responsible for the proper disposal of the local parameterization. +class ParameterBlock { + public: + ParameterBlock(double* user_state, int size) { + Init(user_state, size, NULL); + } + ParameterBlock(double* user_state, + int size, + LocalParameterization* local_parameterization) { + Init(user_state, size, local_parameterization); + } + + // The size of the parameter block. + int Size() const { return size_; } + + // Manipulate the parameter state. + bool SetState(const double* x) { + CHECK(x != NULL) + << "Tried to set the state of constant parameter " + << "with user location " << user_state_; + CHECK(!is_constant_) + << "Tried to set the state of constant parameter " + << "with user location " << user_state_; + + state_ = x; + return UpdateLocalParameterizationJacobian(); + } + + // Copy the current parameter state out to x. This is "GetState()" rather than + // simply "state()" since it is actively copying the data into the passed + // pointer. + void GetState(double *x) const { + if (x != state_) { + memcpy(x, state_, sizeof(*state_) * size_); + } + } + + // Direct pointers to the current state. + const double* state() const { return state_; } + const double* user_state() const { return user_state_; } + double* mutable_user_state() { return user_state_; } + LocalParameterization* local_parameterization() const { + return local_parameterization_; + } + LocalParameterization* mutable_local_parameterization() { + return local_parameterization_; + } + + // Set this parameter block to vary or not. + void SetConstant() { is_constant_ = true; } + void SetVarying() { is_constant_ = false; } + bool IsConstant() const { return is_constant_; } + + // This parameter block's index in an array. + int index() const { return index_; } + void set_index(int index) { index_ = index; } + + // This parameter offset inside a larger state vector. + int state_offset() const { return state_offset_; } + void set_state_offset(int state_offset) { state_offset_ = state_offset; } + + // This parameter offset inside a larger delta vector. + int delta_offset() const { return delta_offset_; } + void set_delta_offset(int delta_offset) { delta_offset_ = delta_offset; } + + // Methods relating to the parameter block's parameterization. + + // The local to global jacobian. Returns NULL if there is no local + // parameterization for this parameter block. The returned matrix is row-major + // and has Size() rows and LocalSize() columns. + const double* LocalParameterizationJacobian() const { + return local_parameterization_jacobian_.get(); + } + + int LocalSize() const { + return (local_parameterization_ == NULL) + ? size_ + : local_parameterization_->LocalSize(); + } + + // Set the parameterization. The parameterization can be set exactly once; + // multiple calls to set the parameterization to different values will crash. + // It is an error to pass NULL for the parameterization. The parameter block + // does not take ownership of the parameterization. + void SetParameterization(LocalParameterization* new_parameterization) { + CHECK(new_parameterization != NULL) << "NULL parameterization invalid."; + CHECK(new_parameterization->GlobalSize() == size_) + << "Invalid parameterization for parameter block. The parameter block " + << "has size " << size_ << " while the parameterization has a global " + << "size of " << new_parameterization->GlobalSize() << ". Did you " + << "accidentally use the wrong parameter block or parameterization?"; + if (new_parameterization != local_parameterization_) { + CHECK(local_parameterization_ == NULL) + << "Can't re-set the local parameterization; it leads to " + << "ambiguous ownership."; + local_parameterization_ = new_parameterization; + local_parameterization_jacobian_.reset( + new double[local_parameterization_->GlobalSize() * + local_parameterization_->LocalSize()]); + CHECK(UpdateLocalParameterizationJacobian()) + "Local parameterization Jacobian computation failed" + "for x: " << ConstVectorRef(state_, Size()).transpose(); + } else { + // Ignore the case that the parameterizations match. + } + } + + // Generalization of the addition operation. This is the same as + // LocalParameterization::Plus() but uses the parameter's current state + // instead of operating on a passed in pointer. + bool Plus(const double *x, const double* delta, double* x_plus_delta) { + if (local_parameterization_ == NULL) { + VectorRef(x_plus_delta, size_) = ConstVectorRef(x, size_) + + ConstVectorRef(delta, size_); + return true; + } + return local_parameterization_->Plus(x, delta, x_plus_delta); + } + + private: + void Init(double* user_state, + int size, + LocalParameterization* local_parameterization) { + user_state_ = user_state; + size_ = size; + is_constant_ = false; + state_ = user_state_; + + local_parameterization_ = NULL; + if (local_parameterization != NULL) { + SetParameterization(local_parameterization); + } + + index_ = -1; + state_offset_ = -1; + delta_offset_ = -1; + } + + bool UpdateLocalParameterizationJacobian() { + if (local_parameterization_ == NULL) { + return true; + } + + // Update the local to global Jacobian. In some cases this is + // wasted effort; if this is a bottleneck, we will find a solution + // at that time. + + const int jacobian_size = Size() * LocalSize(); + InvalidateArray(jacobian_size, + local_parameterization_jacobian_.get()); + if (!local_parameterization_->ComputeJacobian( + state_, + local_parameterization_jacobian_.get())) { + LOG(WARNING) << "Local parameterization Jacobian computation failed" + "for x: " << ConstVectorRef(state_, Size()).transpose(); + return false; + } + + if (!IsArrayValid(jacobian_size, local_parameterization_jacobian_.get())) { + LOG(WARNING) << "Local parameterization Jacobian computation returned" + << "an invalid matrix for x: " + << ConstVectorRef(state_, Size()).transpose() + << "\n Jacobian matrix : " + << ConstMatrixRef(local_parameterization_jacobian_.get(), + Size(), + LocalSize()); + return false; + } + return true; + } + + double* user_state_; + int size_; + bool is_constant_; + LocalParameterization* local_parameterization_; + + // The "state" of the parameter. These fields are only needed while the + // solver is running. While at first glance using mutable is a bad idea, this + // ends up simplifying the internals of Ceres enough to justify the potential + // pitfalls of using "mutable." + mutable const double* state_; + mutable scoped_array<double> local_parameterization_jacobian_; + + // The index of the parameter. This is used by various other parts of Ceres to + // permit switching from a ParameterBlock* to an index in another array. + int32 index_; + + // The offset of this parameter block inside a larger state vector. + int32 state_offset_; + + // The offset of this parameter block inside a larger delta vector. + int32 delta_offset_; + + // Necessary so ProblemImpl can clean up the parameterizations. + friend class ProblemImpl; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_PARAMETER_BLOCK_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/partitioned_matrix_view.cc b/extern/libmv/third_party/ceres/internal/ceres/partitioned_matrix_view.cc new file mode 100644 index 00000000000..fcf8fd53aed --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/partitioned_matrix_view.cc @@ -0,0 +1,315 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#define EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD 10 + +#include "ceres/partitioned_matrix_view.h" + +#include <algorithm> +#include <cstring> +#include <vector> +#include <glog/logging.h> +#include "ceres/block_sparse_matrix.h" +#include "ceres/block_structure.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +PartitionedMatrixView::PartitionedMatrixView( + const BlockSparseMatrixBase& matrix, + int num_col_blocks_a) + : matrix_(matrix), + num_col_blocks_e_(num_col_blocks_a) { + const CompressedRowBlockStructure* bs = matrix_.block_structure(); + CHECK_NOTNULL(bs); + + num_col_blocks_f_ = bs->cols.size() - num_col_blocks_a; + + // Compute the number of row blocks in E. The number of row blocks + // in E maybe less than the number of row blocks in the input matrix + // as some of the row blocks at the bottom may not have any + // e_blocks. For a definition of what an e_block is, please see + // explicit_schur_complement_solver.h + num_row_blocks_e_ = 0; + for (int r = 0; r < bs->rows.size(); ++r) { + const vector<Cell>& cells = bs->rows[r].cells; + if (cells[0].block_id < num_col_blocks_a) { + ++num_row_blocks_e_; + } + } + + // Compute the number of columns in E and F. + num_cols_e_ = 0; + num_cols_f_ = 0; + + for (int c = 0; c < bs->cols.size(); ++c) { + const Block& block = bs->cols[c]; + if (c < num_col_blocks_a) { + num_cols_e_ += block.size; + } else { + num_cols_f_ += block.size; + } + } + + CHECK_EQ(num_cols_e_ + num_cols_f_, matrix_.num_cols()); +} + +PartitionedMatrixView::~PartitionedMatrixView() { +} + +// The next four methods don't seem to be particularly cache +// friendly. This is an artifact of how the BlockStructure of the +// input matrix is constructed. These methods will benefit from +// multithreading as well as improved data layout. + +void PartitionedMatrixView::RightMultiplyE(const double* x, double* y) const { + const CompressedRowBlockStructure* bs = matrix_.block_structure(); + + // Iterate over the first num_row_blocks_e_ row blocks, and multiply + // by the first cell in each row block. + for (int r = 0; r < num_row_blocks_e_; ++r) { + const double* row_values = matrix_.RowBlockValues(r); + const Cell& cell = bs->rows[r].cells[0]; + const int row_block_pos = bs->rows[r].block.position; + const int row_block_size = bs->rows[r].block.size; + const int col_block_id = cell.block_id; + const int col_block_pos = bs->cols[col_block_id].position; + const int col_block_size = bs->cols[col_block_id].size; + + ConstVectorRef xref(x + col_block_pos, col_block_size); + VectorRef yref(y + row_block_pos, row_block_size); + ConstMatrixRef m(row_values + cell.position, + row_block_size, + col_block_size); + yref += m.lazyProduct(xref); + } +} + +void PartitionedMatrixView::RightMultiplyF(const double* x, double* y) const { + const CompressedRowBlockStructure* bs = matrix_.block_structure(); + + // Iterate over row blocks, and if the row block is in E, then + // multiply by all the cells except the first one which is of type + // E. If the row block is not in E (i.e its in the bottom + // num_row_blocks - num_row_blocks_e row blocks), then all the cells + // are of type F and multiply by them all. + for (int r = 0; r < bs->rows.size(); ++r) { + const int row_block_pos = bs->rows[r].block.position; + const int row_block_size = bs->rows[r].block.size; + VectorRef yref(y + row_block_pos, row_block_size); + const vector<Cell>& cells = bs->rows[r].cells; + for (int c = (r < num_row_blocks_e_) ? 1 : 0; c < cells.size(); ++c) { + const double* row_values = matrix_.RowBlockValues(r); + const int col_block_id = cells[c].block_id; + const int col_block_pos = bs->cols[col_block_id].position; + const int col_block_size = bs->cols[col_block_id].size; + + ConstVectorRef xref(x + col_block_pos - num_cols_e(), + col_block_size); + ConstMatrixRef m(row_values + cells[c].position, + row_block_size, + col_block_size); + yref += m.lazyProduct(xref); + } + } +} + +void PartitionedMatrixView::LeftMultiplyE(const double* x, double* y) const { + const CompressedRowBlockStructure* bs = matrix_.block_structure(); + + // Iterate over the first num_row_blocks_e_ row blocks, and multiply + // by the first cell in each row block. + for (int r = 0; r < num_row_blocks_e_; ++r) { + const Cell& cell = bs->rows[r].cells[0]; + const double* row_values = matrix_.RowBlockValues(r); + const int row_block_pos = bs->rows[r].block.position; + const int row_block_size = bs->rows[r].block.size; + const int col_block_id = cell.block_id; + const int col_block_pos = bs->cols[col_block_id].position; + const int col_block_size = bs->cols[col_block_id].size; + + ConstVectorRef xref(x + row_block_pos, row_block_size); + VectorRef yref(y + col_block_pos, col_block_size); + ConstMatrixRef m(row_values + cell.position, + row_block_size, + col_block_size); + yref += m.transpose().lazyProduct(xref); + } +} + +void PartitionedMatrixView::LeftMultiplyF(const double* x, double* y) const { + const CompressedRowBlockStructure* bs = matrix_.block_structure(); + + // Iterate over row blocks, and if the row block is in E, then + // multiply by all the cells except the first one which is of type + // E. If the row block is not in E (i.e its in the bottom + // num_row_blocks - num_row_blocks_e row blocks), then all the cells + // are of type F and multiply by them all. + for (int r = 0; r < bs->rows.size(); ++r) { + const int row_block_pos = bs->rows[r].block.position; + const int row_block_size = bs->rows[r].block.size; + ConstVectorRef xref(x + row_block_pos, row_block_size); + const vector<Cell>& cells = bs->rows[r].cells; + for (int c = (r < num_row_blocks_e_) ? 1 : 0; c < cells.size(); ++c) { + const double* row_values = matrix_.RowBlockValues(r); + const int col_block_id = cells[c].block_id; + const int col_block_pos = bs->cols[col_block_id].position; + const int col_block_size = bs->cols[col_block_id].size; + + VectorRef yref(y + col_block_pos - num_cols_e(), col_block_size); + ConstMatrixRef m(row_values + cells[c].position, + row_block_size, + col_block_size); + yref += m.transpose().lazyProduct(xref); + } + } +} + +// Given a range of columns blocks of a matrix m, compute the block +// structure of the block diagonal of the matrix m(:, +// start_col_block:end_col_block)'m(:, start_col_block:end_col_block) +// and return a BlockSparseMatrix with the this block structure. The +// caller owns the result. +BlockSparseMatrix* PartitionedMatrixView::CreateBlockDiagonalMatrixLayout( + int start_col_block, int end_col_block) const { + const CompressedRowBlockStructure* bs = matrix_.block_structure(); + CompressedRowBlockStructure* block_diagonal_structure = + new CompressedRowBlockStructure; + + int block_position = 0; + int diagonal_cell_position = 0; + + // Iterate over the column blocks, creating a new diagonal block for + // each column block. + for (int c = start_col_block; c < end_col_block; ++c) { + const Block& block = bs->cols[c]; + block_diagonal_structure->cols.push_back(Block()); + Block& diagonal_block = block_diagonal_structure->cols.back(); + diagonal_block.size = block.size; + diagonal_block.position = block_position; + + block_diagonal_structure->rows.push_back(CompressedRow()); + CompressedRow& row = block_diagonal_structure->rows.back(); + row.block = diagonal_block; + + row.cells.push_back(Cell()); + Cell& cell = row.cells.back(); + cell.block_id = c - start_col_block; + cell.position = diagonal_cell_position; + + block_position += block.size; + diagonal_cell_position += block.size * block.size; + } + + // Build a BlockSparseMatrix with the just computed block + // structure. + return new BlockSparseMatrix(block_diagonal_structure); +} + +BlockSparseMatrix* PartitionedMatrixView::CreateBlockDiagonalEtE() const { + BlockSparseMatrix* block_diagonal = + CreateBlockDiagonalMatrixLayout(0, num_col_blocks_e_); + UpdateBlockDiagonalEtE(block_diagonal); + return block_diagonal; +} + +BlockSparseMatrix* PartitionedMatrixView::CreateBlockDiagonalFtF() const { + BlockSparseMatrix* block_diagonal = + CreateBlockDiagonalMatrixLayout( + num_col_blocks_e_, num_col_blocks_e_ + num_col_blocks_f_); + UpdateBlockDiagonalFtF(block_diagonal); + return block_diagonal; +} + +// Similar to the code in RightMultiplyE, except instead of the matrix +// vector multiply its an outer product. +// +// block_diagonal = block_diagonal(E'E) +void PartitionedMatrixView::UpdateBlockDiagonalEtE( + BlockSparseMatrix* block_diagonal) const { + const CompressedRowBlockStructure* bs = matrix_.block_structure(); + const CompressedRowBlockStructure* block_diagonal_structure = + block_diagonal->block_structure(); + + block_diagonal->SetZero(); + + for (int r = 0; r < num_row_blocks_e_ ; ++r) { + const double* row_values = matrix_.RowBlockValues(r); + const Cell& cell = bs->rows[r].cells[0]; + const int row_block_size = bs->rows[r].block.size; + const int block_id = cell.block_id; + const int col_block_size = bs->cols[block_id].size; + ConstMatrixRef m(row_values + cell.position, + row_block_size, + col_block_size); + + const int cell_position = + block_diagonal_structure->rows[block_id].cells[0].position; + + MatrixRef(block_diagonal->mutable_values() + cell_position, + col_block_size, col_block_size).noalias() += m.transpose() * m; + } +} + +// Similar to the code in RightMultiplyF, except instead of the matrix +// vector multiply its an outer product. +// +// block_diagonal = block_diagonal(F'F) +// +void PartitionedMatrixView::UpdateBlockDiagonalFtF( + BlockSparseMatrix* block_diagonal) const { + const CompressedRowBlockStructure* bs = matrix_.block_structure(); + const CompressedRowBlockStructure* block_diagonal_structure = + block_diagonal->block_structure(); + + block_diagonal->SetZero(); + for (int r = 0; r < bs->rows.size(); ++r) { + const int row_block_size = bs->rows[r].block.size; + const vector<Cell>& cells = bs->rows[r].cells; + const double* row_values = matrix_.RowBlockValues(r); + for (int c = (r < num_row_blocks_e_) ? 1 : 0; c < cells.size(); ++c) { + const int col_block_id = cells[c].block_id; + const int col_block_size = bs->cols[col_block_id].size; + ConstMatrixRef m(row_values + cells[c].position, + row_block_size, + col_block_size); + const int diagonal_block_id = col_block_id - num_col_blocks_e_; + const int cell_position = + block_diagonal_structure->rows[diagonal_block_id].cells[0].position; + + MatrixRef(block_diagonal->mutable_values() + cell_position, + col_block_size, col_block_size).noalias() += m.transpose() * m; + } + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/partitioned_matrix_view.h b/extern/libmv/third_party/ceres/internal/ceres/partitioned_matrix_view.h new file mode 100644 index 00000000000..cfe4de5b436 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/partitioned_matrix_view.h @@ -0,0 +1,121 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// For generalized bi-partite Jacobian matrices that arise in +// Structure from Motion related problems, it is sometimes useful to +// have access to the two parts of the matrix as linear operators +// themselves. This class provides that functionality. + +#ifndef CERES_INTERNAL_PARTITIONED_MATRIX_VIEW_H_ +#define CERES_INTERNAL_PARTITIONED_MATRIX_VIEW_H_ + +#include "ceres/block_sparse_matrix.h" + +namespace ceres { +namespace internal { + +// Given generalized bi-partite matrix A = [E F], with the same block +// structure as required by the Schur complement based solver, found +// in explicit_schur_complement_solver.h, provide access to the +// matrices E and F and their outer products E'E and F'F with +// themselves. +// +// Lack of BlockStructure object will result in a crash and if the +// block structure of the matrix does not satisfy the requirements of +// the Schur complement solver it will result in unpredictable and +// wrong output. +// +// This class lives in the internal name space as its a utility class +// to be used by the IterativeSchurComplementSolver class, found in +// iterative_schur_complement_solver.h, and is not meant for general +// consumption. +class PartitionedMatrixView { + public: + // matrix = [E F], where the matrix E contains the first + // num_col_blocks_a column blocks. + PartitionedMatrixView(const BlockSparseMatrixBase& matrix, + int num_col_blocks_a); + ~PartitionedMatrixView(); + + // y += E'x + void LeftMultiplyE(const double* x, double* y) const; + + // y += F'x + void LeftMultiplyF(const double* x, double* y) const; + + // y += Ex + void RightMultiplyE(const double* x, double* y) const; + + // y += Fx + void RightMultiplyF(const double* x, double* y) const; + + // Create and return the block diagonal of the matrix E'E. + BlockSparseMatrix* CreateBlockDiagonalEtE() const; + + // Create and return the block diagonal of the matrix F'F. + BlockSparseMatrix* CreateBlockDiagonalFtF() const; + + // Compute the block diagonal of the matrix E'E and store it in + // block_diagonal. The matrix block_diagonal is expected to have a + // BlockStructure (preferably created using + // CreateBlockDiagonalMatrixEtE) which is has the same structure as + // the block diagonal of E'E. + void UpdateBlockDiagonalEtE(BlockSparseMatrix* block_diagonal) const; + + // Compute the block diagonal of the matrix F'F and store it in + // block_diagonal. The matrix block_diagonal is expected to have a + // BlockStructure (preferably created using + // CreateBlockDiagonalMatrixFtF) which is has the same structure as + // the block diagonal of F'F. + void UpdateBlockDiagonalFtF(BlockSparseMatrix* block_diagonal) const; + + int num_col_blocks_e() const { return num_col_blocks_e_; } + int num_col_blocks_f() const { return num_col_blocks_f_; } + int num_cols_e() const { return num_cols_e_; } + int num_cols_f() const { return num_cols_f_; } + int num_rows() const { return matrix_.num_rows(); } + int num_cols() const { return matrix_.num_cols(); } + + private: + BlockSparseMatrix* CreateBlockDiagonalMatrixLayout(int start_col_block, + int end_col_block) const; + + const BlockSparseMatrixBase& matrix_; + int num_row_blocks_e_; + int num_col_blocks_e_; + int num_col_blocks_f_; + int num_cols_e_; + int num_cols_f_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_PARTITIONED_MATRIX_VIEW_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/problem.cc b/extern/libmv/third_party/ceres/internal/ceres/problem.cc new file mode 100644 index 00000000000..b8c25d9db84 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/problem.cc @@ -0,0 +1,149 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// keir@google.com (Keir Mierle) + +#include "ceres/problem.h" + +#include <vector> +#include "ceres/problem_impl.h" + +namespace ceres { + +class ResidualBlock; + +Problem::Problem() : problem_impl_(new internal::ProblemImpl) {} +Problem::Problem(const Problem::Options& options) + : problem_impl_(new internal::ProblemImpl(options)) {} +Problem::~Problem() {} + +ResidualBlockId Problem::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + const vector<double*>& parameter_blocks) { + return problem_impl_->AddResidualBlock(cost_function, + loss_function, + parameter_blocks); +} + +ResidualBlockId Problem::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0) { + return problem_impl_->AddResidualBlock(cost_function, + loss_function, + x0); +} + +ResidualBlockId Problem::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1) { + return problem_impl_->AddResidualBlock(cost_function, + loss_function, + x0, x1); +} + +ResidualBlockId Problem::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2) { + return problem_impl_->AddResidualBlock(cost_function, + loss_function, + x0, x1, x2); +} + +ResidualBlockId Problem::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, double* x3) { + return problem_impl_->AddResidualBlock(cost_function, + loss_function, + x0, x1, x2, x3); +} + +ResidualBlockId Problem::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, double* x3, double* x4) { + return problem_impl_->AddResidualBlock(cost_function, + loss_function, + x0, x1, x2, x3, x4); +} + +ResidualBlockId Problem::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, double* x3, double* x4, double* x5) { + return problem_impl_->AddResidualBlock(cost_function, + loss_function, + x0, x1, x2, x3, x4, x5); +} + +void Problem::AddParameterBlock(double* values, int size) { + problem_impl_->AddParameterBlock(values, size); +} + +void Problem::AddParameterBlock(double* values, + int size, + LocalParameterization* local_parameterization) { + problem_impl_->AddParameterBlock(values, size, local_parameterization); +} + +void Problem::SetParameterBlockConstant(double* values) { + problem_impl_->SetParameterBlockConstant(values); +} + +void Problem::SetParameterBlockVariable(double* values) { + problem_impl_->SetParameterBlockVariable(values); +} + +void Problem::SetParameterization( + double* values, + LocalParameterization* local_parameterization) { + problem_impl_->SetParameterization(values, local_parameterization); +} + +int Problem::NumParameterBlocks() const { + return problem_impl_->NumParameterBlocks(); +} + +int Problem::NumParameters() const { + return problem_impl_->NumParameters(); +} + +int Problem::NumResidualBlocks() const { + return problem_impl_->NumResidualBlocks(); +} + +int Problem::NumResiduals() const { + return problem_impl_->NumResiduals(); +} + +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/problem_impl.cc b/extern/libmv/third_party/ceres/internal/ceres/problem_impl.cc new file mode 100644 index 00000000000..68242477d6f --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/problem_impl.cc @@ -0,0 +1,359 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// keir@google.com (Keir Mierle) + +#include "ceres/problem_impl.h" + +#include <algorithm> +#include <cstddef> +#include <set> +#include <string> +#include <utility> +#include <vector> + +#include <glog/logging.h> +#include "ceres/parameter_block.h" +#include "ceres/program.h" +#include "ceres/residual_block.h" +#include "ceres/stl_util.h" +#include "ceres/map_util.h" +#include "ceres/stringprintf.h" +#include "ceres/cost_function.h" +#include "ceres/loss_function.h" + +namespace ceres { +namespace internal { + +typedef map<double*, internal::ParameterBlock*> ParameterMap; + +// Returns true if two regions of memory, a and b, with sizes size_a and size_b +// respectively, overlap. +static bool RegionsAlias(const double* a, int size_a, + const double* b, int size_b) { + return (a < b) ? b < (a + size_a) + : a < (b + size_b); +} + +static void CheckForNoAliasing(double* existing_block, + int existing_block_size, + double* new_block, + int new_block_size) { + CHECK(!RegionsAlias(existing_block, existing_block_size, + new_block, new_block_size)) + << "Aliasing detected between existing parameter block at memory " + << "location " << existing_block + << " and has size " << existing_block_size << " with new parameter " + << "block that has memory adderss " << new_block << " and would have " + << "size " << new_block_size << "."; +} + +static ParameterBlock* InternalAddParameterBlock( + double* values, + int size, + ParameterMap* parameter_map, + vector<ParameterBlock*>* parameter_blocks) { + CHECK(values) << "Null pointer passed to AddParameterBlock for a parameter " + << "with size " << size; + + // Ignore the request if there is a block for the given pointer already. + ParameterMap::iterator it = parameter_map->find(values); + if (it != parameter_map->end()) { + int existing_size = it->second->Size(); + CHECK(size == existing_size) + << "Tried adding a parameter block with the same double pointer, " + << values << ", twice, but with different block sizes. Original " + << "size was " << existing_size << " but new size is " + << size; + return it->second; + } + // Before adding the parameter block, also check that it doesn't alias any + // other parameter blocks. + if (!parameter_map->empty()) { + ParameterMap::iterator lb = parameter_map->lower_bound(values); + + // If lb is not the first block, check the previous block for aliasing. + if (lb != parameter_map->begin()) { + ParameterMap::iterator previous = lb; + --previous; + CheckForNoAliasing(previous->first, + previous->second->Size(), + values, + size); + } + + // If lb is not off the end, check lb for aliasing. + if (lb != parameter_map->end()) { + CheckForNoAliasing(lb->first, + lb->second->Size(), + values, + size); + } + } + ParameterBlock* new_parameter_block = new ParameterBlock(values, size); + (*parameter_map)[values] = new_parameter_block; + parameter_blocks->push_back(new_parameter_block); + return new_parameter_block; +} + +ProblemImpl::ProblemImpl() : program_(new internal::Program) {} +ProblemImpl::ProblemImpl(const Problem::Options& options) + : options_(options), + program_(new internal::Program) {} + +ProblemImpl::~ProblemImpl() { + // Collect the unique cost/loss functions and delete the residuals. + set<CostFunction*> cost_functions; + set<LossFunction*> loss_functions; + for (int i = 0; i < program_->residual_blocks_.size(); ++i) { + ResidualBlock* residual_block = program_->residual_blocks_[i]; + + // The const casts here are legit, since ResidualBlock holds these + // pointers as const pointers but we have ownership of them and + // have the right to destroy them when the destructor is called. + if (options_.cost_function_ownership == TAKE_OWNERSHIP) { + cost_functions.insert( + const_cast<CostFunction*>(residual_block->cost_function())); + } + if (options_.loss_function_ownership == TAKE_OWNERSHIP) { + loss_functions.insert( + const_cast<LossFunction*>(residual_block->loss_function())); + } + + delete residual_block; + } + + // Collect the unique parameterizations and delete the parameters. + set<LocalParameterization*> local_parameterizations; + for (int i = 0; i < program_->parameter_blocks_.size(); ++i) { + ParameterBlock* parameter_block = program_->parameter_blocks_[i]; + + if (options_.local_parameterization_ownership == TAKE_OWNERSHIP) { + local_parameterizations.insert(parameter_block->local_parameterization_); + } + + delete parameter_block; + } + + // Delete the owned cost/loss functions and parameterizations. + STLDeleteContainerPointers(local_parameterizations.begin(), + local_parameterizations.end()); + STLDeleteContainerPointers(cost_functions.begin(), + cost_functions.end()); + STLDeleteContainerPointers(loss_functions.begin(), + loss_functions.end()); +} + +const ResidualBlock* ProblemImpl::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + const vector<double*>& parameter_blocks) { + CHECK_NOTNULL(cost_function); + CHECK_EQ(parameter_blocks.size(), + cost_function->parameter_block_sizes().size()); + + // Check the sizes match. + const vector<int16>& parameter_block_sizes = + cost_function->parameter_block_sizes(); + CHECK_EQ(parameter_block_sizes.size(), parameter_blocks.size()) + << "Number of blocks input is different than the number of blocks " + << "that the cost function expects."; + + // Check for duplicate parameter blocks. + vector<double*> sorted_parameter_blocks(parameter_blocks); + sort(sorted_parameter_blocks.begin(), sorted_parameter_blocks.end()); + vector<double*>::const_iterator duplicate_items = + unique(sorted_parameter_blocks.begin(), + sorted_parameter_blocks.end()); + if (duplicate_items != sorted_parameter_blocks.end()) { + string blocks; + for (int i = 0; i < parameter_blocks.size(); ++i) { + blocks += internal::StringPrintf(" %p ", parameter_blocks[i]); + } + + LOG(FATAL) << "Duplicate parameter blocks in a residual parameter " + << "are not allowed. Parameter block pointers: [" + << blocks << "]"; + } + + // Add parameter blocks and convert the double*'s to parameter blocks. + vector<ParameterBlock*> parameter_block_ptrs(parameter_blocks.size()); + for (int i = 0; i < parameter_blocks.size(); ++i) { + parameter_block_ptrs[i] = + InternalAddParameterBlock(parameter_blocks[i], + parameter_block_sizes[i], + ¶meter_block_map_, + &program_->parameter_blocks_); + } + + // Check that the block sizes match the block sizes expected by the + // cost_function. + for (int i = 0; i < parameter_block_ptrs.size(); ++i) { + CHECK_EQ(cost_function->parameter_block_sizes()[i], + parameter_block_ptrs[i]->Size()) + << "The cost function expects parameter block " << i + << " of size " << cost_function->parameter_block_sizes()[i] + << " but was given a block of size " + << parameter_block_ptrs[i]->Size(); + } + + ResidualBlock* new_residual_block = + new ResidualBlock(cost_function, + loss_function, + parameter_block_ptrs); + program_->residual_blocks_.push_back(new_residual_block); + return new_residual_block; +} + +// Unfortunately, macros don't help much to reduce this code, and var args don't +// work because of the ambiguous case that there is no loss function. +const ResidualBlock* ProblemImpl::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0) { + vector<double*> residual_parameters; + residual_parameters.push_back(x0); + return AddResidualBlock(cost_function, loss_function, residual_parameters); +} + +const ResidualBlock* ProblemImpl::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1) { + vector<double*> residual_parameters; + residual_parameters.push_back(x0); + residual_parameters.push_back(x1); + return AddResidualBlock(cost_function, loss_function, residual_parameters); +} + +const ResidualBlock* ProblemImpl::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2) { + vector<double*> residual_parameters; + residual_parameters.push_back(x0); + residual_parameters.push_back(x1); + residual_parameters.push_back(x2); + return AddResidualBlock(cost_function, loss_function, residual_parameters); +} + +const ResidualBlock* ProblemImpl::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, double* x3) { + vector<double*> residual_parameters; + residual_parameters.push_back(x0); + residual_parameters.push_back(x1); + residual_parameters.push_back(x2); + residual_parameters.push_back(x3); + return AddResidualBlock(cost_function, loss_function, residual_parameters); +} + +const ResidualBlock* ProblemImpl::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, double* x3, double* x4) { + vector<double*> residual_parameters; + residual_parameters.push_back(x0); + residual_parameters.push_back(x1); + residual_parameters.push_back(x2); + residual_parameters.push_back(x3); + residual_parameters.push_back(x4); + return AddResidualBlock(cost_function, loss_function, residual_parameters); +} + +const ResidualBlock* ProblemImpl::AddResidualBlock( + CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, double* x3, double* x4, double* x5) { + vector<double*> residual_parameters; + residual_parameters.push_back(x0); + residual_parameters.push_back(x1); + residual_parameters.push_back(x2); + residual_parameters.push_back(x3); + residual_parameters.push_back(x4); + residual_parameters.push_back(x5); + return AddResidualBlock(cost_function, loss_function, residual_parameters); +} + + +void ProblemImpl::AddParameterBlock(double* values, int size) { + InternalAddParameterBlock(values, + size, + ¶meter_block_map_, + &program_->parameter_blocks_); +} + +void ProblemImpl::AddParameterBlock( + double* values, + int size, + LocalParameterization* local_parameterization) { + ParameterBlock* parameter_block = + InternalAddParameterBlock(values, + size, + ¶meter_block_map_, + &program_->parameter_blocks_); + if (local_parameterization != NULL) { + parameter_block->SetParameterization(local_parameterization); + } +} + +void ProblemImpl::SetParameterBlockConstant(double* values) { + FindOrDie(parameter_block_map_, values)->SetConstant(); +} + +void ProblemImpl::SetParameterBlockVariable(double* values) { + FindOrDie(parameter_block_map_, values)->SetVarying(); +} + +void ProblemImpl::SetParameterization( + double* values, + LocalParameterization* local_parameterization) { + FindOrDie(parameter_block_map_, values) + ->SetParameterization(local_parameterization); +} + +int ProblemImpl::NumParameterBlocks() const { + return program_->NumParameterBlocks(); +} + +int ProblemImpl::NumParameters() const { + return program_->NumParameters(); +} + +int ProblemImpl::NumResidualBlocks() const { + return program_->NumResidualBlocks(); +} + +int ProblemImpl::NumResiduals() const { + return program_->NumResiduals(); +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/problem_impl.h b/extern/libmv/third_party/ceres/internal/ceres/problem_impl.h new file mode 100644 index 00000000000..523860e652a --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/problem_impl.h @@ -0,0 +1,127 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// This is the implementation of the public Problem API. The pointer to +// implementation (PIMPL) idiom makes it possible for Ceres internal code to +// refer to the private data members without needing to exposing it to the +// world. An alternative to PIMPL is to have a factory which returns instances +// of a virtual base class; while that approach would work, it requires clients +// to always put a Problem object into a scoped pointer; this needlessly muddies +// client code for little benefit. Therefore, the PIMPL comprise was chosen. + +#ifndef CERES_PUBLIC_PROBLEM_IMPL_H_ +#define CERES_PUBLIC_PROBLEM_IMPL_H_ + +#include <map> +#include <vector> + +#include "ceres/internal/macros.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/problem.h" +#include "ceres/types.h" + +namespace ceres { + +class CostFunction; +class LossFunction; +class LocalParameterization; + +namespace internal { + +class Program; +class ResidualBlock; + +class ProblemImpl { + public: + typedef map<double*, ParameterBlock*> ParameterMap; + + ProblemImpl(); + explicit ProblemImpl(const Problem::Options& options); + + ~ProblemImpl(); + + // See the public problem.h file for description of these methods. + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + const vector<double*>& parameter_blocks); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, + double* x3); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, + double* x3, double* x4); + ResidualBlockId AddResidualBlock(CostFunction* cost_function, + LossFunction* loss_function, + double* x0, double* x1, double* x2, + double* x3, double* x4, double* x5); + void AddParameterBlock(double* values, int size); + void AddParameterBlock(double* values, + int size, + LocalParameterization* local_parameterization); + void SetParameterBlockConstant(double* values); + void SetParameterBlockVariable(double* values); + void SetParameterization(double* values, + LocalParameterization* local_parameterization); + int NumParameterBlocks() const; + int NumParameters() const; + int NumResidualBlocks() const; + int NumResiduals() const; + + const Program& program() const { return *program_; } + Program* mutable_program() { return program_.get(); } + + const ParameterMap& parameter_map() const { return parameter_block_map_; } + + private: + const Problem::Options options_; + + // The mapping from user pointers to parameter blocks. + map<double*, ParameterBlock*> parameter_block_map_; + + internal::scoped_ptr<internal::Program> program_; + DISALLOW_COPY_AND_ASSIGN(ProblemImpl); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_PUBLIC_PROBLEM_IMPL_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/program.cc b/extern/libmv/third_party/ceres/internal/ceres/program.cc new file mode 100644 index 00000000000..444b1020253 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/program.cc @@ -0,0 +1,233 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#include "ceres/program.h" + +#include <map> +#include <vector> +#include "ceres/parameter_block.h" +#include "ceres/residual_block.h" +#include "ceres/stl_util.h" +#include "ceres/map_util.h" +#include "ceres/problem.h" +#include "ceres/cost_function.h" +#include "ceres/loss_function.h" +#include "ceres/local_parameterization.h" + +namespace ceres { +namespace internal { + +Program::Program() {} + +Program::Program(const Program& program) + : parameter_blocks_(program.parameter_blocks_), + residual_blocks_(program.residual_blocks_) { +} + +const vector<ParameterBlock*>& Program::parameter_blocks() const { + return parameter_blocks_; +} + +const vector<ResidualBlock*>& Program::residual_blocks() const { + return residual_blocks_; +} + +vector<ParameterBlock*>* Program::mutable_parameter_blocks() { + return ¶meter_blocks_; +} + +vector<ResidualBlock*>* Program::mutable_residual_blocks() { + return &residual_blocks_; +} + +bool Program::StateVectorToParameterBlocks(const double *state) { + for (int i = 0; i < parameter_blocks_.size(); ++i) { + if (!parameter_blocks_[i]->SetState(state)) { + return false; + } + state += parameter_blocks_[i]->Size(); + } + return true; +} + +void Program::ParameterBlocksToStateVector(double *state) const { + for (int i = 0; i < parameter_blocks_.size(); ++i) { + parameter_blocks_[i]->GetState(state); + state += parameter_blocks_[i]->Size(); + } +} + +void Program::CopyParameterBlockStateToUserState() { + for (int i = 0; i < parameter_blocks_.size(); ++i) { + parameter_blocks_[i]->GetState( + parameter_blocks_[i]->mutable_user_state()); + } +} + +bool Program::Plus(const double* state, + const double* delta, + double* state_plus_delta) const { + for (int i = 0; i < parameter_blocks_.size(); ++i) { + if (!parameter_blocks_[i]->Plus(state, delta, state_plus_delta)) { + return false; + } + state += parameter_blocks_[i]->Size(); + delta += parameter_blocks_[i]->LocalSize(); + state_plus_delta += parameter_blocks_[i]->Size(); + } + return true; +} + +void Program::SetParameterOffsetsAndIndex() { + // Set positions for all parameters appearing as arguments to residuals to one + // past the end of the parameter block array. + for (int i = 0; i < residual_blocks_.size(); ++i) { + ResidualBlock* residual_block = residual_blocks_[i]; + for (int j = 0; j < residual_block->NumParameterBlocks(); ++j) { + residual_block->parameter_blocks()[j]->set_index(-1); + } + } + // For parameters that appear in the program, set their position and offset. + int state_offset = 0; + int delta_offset = 0; + for (int i = 0; i < parameter_blocks_.size(); ++i) { + parameter_blocks_[i]->set_index(i); + parameter_blocks_[i]->set_state_offset(state_offset); + parameter_blocks_[i]->set_delta_offset(delta_offset); + state_offset += parameter_blocks_[i]->Size(); + delta_offset += parameter_blocks_[i]->LocalSize(); + } +} + +int Program::NumResidualBlocks() const { + return residual_blocks_.size(); +} + +int Program::NumParameterBlocks() const { + return parameter_blocks_.size(); +} + +int Program::NumResiduals() const { + int num_residuals = 0; + for (int i = 0; i < residual_blocks_.size(); ++i) { + num_residuals += residual_blocks_[i]->NumResiduals(); + } + return num_residuals; +} + +int Program::NumParameters() const { + int num_parameters = 0; + for (int i = 0; i < parameter_blocks_.size(); ++i) { + num_parameters += parameter_blocks_[i]->Size(); + } + return num_parameters; +} + +int Program::NumEffectiveParameters() const { + int num_parameters = 0; + for (int i = 0; i < parameter_blocks_.size(); ++i) { + num_parameters += parameter_blocks_[i]->LocalSize(); + } + return num_parameters; +} + +int Program::MaxScratchDoublesNeededForEvaluate() const { + // Compute the scratch space needed for evaluate. + int max_scratch_bytes_for_evaluate = 0; + for (int i = 0; i < residual_blocks_.size(); ++i) { + max_scratch_bytes_for_evaluate = + max(max_scratch_bytes_for_evaluate, + residual_blocks_[i]->NumScratchDoublesForEvaluate()); + } + return max_scratch_bytes_for_evaluate; +} + +int Program::MaxDerivativesPerResidualBlock() const { + int max_derivatives = 0; + for (int i = 0; i < residual_blocks_.size(); ++i) { + int derivatives = 0; + ResidualBlock* residual_block = residual_blocks_[i]; + int num_parameters = residual_block->NumParameterBlocks(); + for (int j = 0; j < num_parameters; ++j) { + derivatives += residual_block->NumResiduals() * + residual_block->parameter_blocks()[j]->LocalSize(); + } + max_derivatives = max(max_derivatives, derivatives); + } + return max_derivatives; +} + +int Program::MaxParametersPerResidualBlock() const { + int max_parameters = 0; + for (int i = 0; i < residual_blocks_.size(); ++i) { + max_parameters = max(max_parameters, + residual_blocks_[i]->NumParameterBlocks()); + } + return max_parameters; +} + +bool Program::Evaluate(double* cost, double* residuals) { + *cost = 0.0; + + // Scratch space is only needed if residuals is NULL. + scoped_array<double> scratch; + if (residuals == NULL) { + scratch.reset(new double[MaxScratchDoublesNeededForEvaluate()]); + } else { + // TODO(keir): Is this needed? Check by removing the equivalent statement in + // dense_evaluator.cc and running the tests. + VectorRef(residuals, NumResiduals()).setZero(); + } + + for (int i = 0; i < residual_blocks_.size(); ++i) { + ResidualBlock* residual_block = residual_blocks_[i]; + + // Evaluate the cost function for this residual. + double residual_cost; + if (!residual_block->Evaluate(&residual_cost, + residuals, + NULL, // No jacobian. + scratch.get())) { + return false; + } + + // Accumulate residual cost into the total cost. + *cost += residual_cost; + + // Update the residuals cursor. + if (residuals != NULL) { + residuals += residual_block->NumResiduals(); + } + } + return true; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/program.h b/extern/libmv/third_party/ceres/internal/ceres/program.h new file mode 100644 index 00000000000..113d352d562 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/program.h @@ -0,0 +1,128 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#ifndef CERES_INTERNAL_PROGRAM_H_ +#define CERES_INTERNAL_PROGRAM_H_ + +#include <vector> +#include "ceres/internal/port.h" + +namespace ceres { +namespace internal { + +class ParameterBlock; +class ProblemImpl; +class ResidualBlock; + +// A nonlinear least squares optimization problem. This is different from the +// similarly-named "Problem" object, which offers a mutation interface for +// adding and modifying parameters and residuals. The Program contains the core +// part of the Problem, which is the parameters and the residuals, stored in a +// particular ordering. The ordering is critical, since it defines the mapping +// between (residual, parameter) pairs and a position in the jacobian of the +// objective function. Various parts of Ceres transform one Program into +// another; for example, the first stage of solving involves stripping all +// constant parameters and residuals. This is in contrast with Problem, which is +// not built for transformation. +class Program { + public: + Program(); + explicit Program(const Program& program); + + // The ordered parameter and residual blocks for the program. + const vector<ParameterBlock*>& parameter_blocks() const; + const vector<ResidualBlock*>& residual_blocks() const; + vector<ParameterBlock*>* mutable_parameter_blocks(); + vector<ResidualBlock*>* mutable_residual_blocks(); + + // Serialize to/from the program and update states. + // + // NOTE: Setting the state of a parameter block can trigger the + // computation of the Jacobian of its local parameterization. If + // this computation fails for some reason, then this method returns + // false and the state of the parameter blocks cannot be trusted. + bool StateVectorToParameterBlocks(const double *state); + void ParameterBlocksToStateVector(double *state) const; + + // Copy internal state out to the user's parameters. + void CopyParameterBlockStateToUserState(); + + // Update a state vector for the program given a delta. + bool Plus(const double* state, + const double* delta, + double* state_plus_delta) const; + + // Set the parameter indices and offsets. This permits mapping backward + // from a ParameterBlock* to an index in the parameter_blocks() vector. For + // any parameter block p, after calling SetParameterOffsetsAndIndex(), it + // is true that + // + // parameter_blocks()[p->index()] == p + // + // If a parameter appears in a residual but not in the parameter block, then + // it will have an index of -1. + // + // This also updates p->state_offset() and p->delta_offset(), which are the + // position of the parameter in the state and delta vector respectively. + void SetParameterOffsetsAndIndex(); + + // See problem.h for what these do. + int NumParameterBlocks() const; + int NumParameters() const; + int NumEffectiveParameters() const; + int NumResidualBlocks() const; + int NumResiduals() const; + + int MaxScratchDoublesNeededForEvaluate() const; + int MaxDerivativesPerResidualBlock() const; + int MaxParametersPerResidualBlock() const; + + // Evaluate the cost and maybe the residuals for the program. If residuals is + // NULL, then residuals are not calculated. If the jacobian is needed, instead + // use the various evaluators (e.g. dense_evaluator.h). + // + // This is a trivial implementation of evaluate not intended for use in the + // core solving loop. The other evaluators, which support constructing the + // jacobian in addition to the cost and residuals, are considerably + // complicated by the need to construct the jacobian. + bool Evaluate(double* cost, double* residuals); + + private: + // The Program does not own the ParameterBlock or ResidualBlock objects. + vector<ParameterBlock*> parameter_blocks_; + vector<ResidualBlock*> residual_blocks_; + + friend class ProblemImpl; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_PROGRAM_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/program_evaluator.h b/extern/libmv/third_party/ceres/internal/ceres/program_evaluator.h new file mode 100644 index 00000000000..7ec74b1b269 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/program_evaluator.h @@ -0,0 +1,279 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// The ProgramEvaluator runs the cost functions contained in each residual block +// and stores the result into a jacobian. The particular type of jacobian is +// abstracted out using two template parameters: +// +// - An "EvaluatePreparer" that is responsible for creating the array with +// pointers to the jacobian blocks where the cost function evaluates to. +// - A "JacobianWriter" that is responsible for storing the resulting +// jacobian blocks in the passed sparse matrix. +// +// This abstraction affords an efficient evaluator implementation while still +// supporting writing to multiple sparse matrix formats. For example, when the +// ProgramEvaluator is parameterized for writing to block sparse matrices, the +// residual jacobians are written directly into their final position in the +// block sparse matrix by the user's CostFunction; there is no copying. +// +// The evaluation is threaded with OpenMP. +// +// The EvaluatePreparer and JacobianWriter interfaces are as follows: +// +// class EvaluatePreparer { +// // Prepare the jacobians array for use as the destination of a call to +// // a cost function's evaluate method. +// void Prepare(const ResidualBlock* residual_block, +// int residual_block_index, +// SparseMatrix* jacobian, +// double** jacobians); +// } +// +// class JacobianWriter { +// // Create a jacobian that this writer can write. Same as +// // Evaluator::CreateJacobian. +// SparseMatrix* CreateJacobian() const; +// +// // Create num_threads evaluate preparers. Caller owns result which must +// // be freed with delete[]. Resulting preparers are valid while *this is. +// EvaluatePreparer* CreateEvaluatePreparers(int num_threads); +// +// // Write the block jacobians from a residual block evaluation to the +// // larger sparse jacobian. +// void Write(int residual_id, +// int residual_offset, +// double** jacobians, +// SparseMatrix* jacobian); +// } +// +// Note: The ProgramEvaluator is not thread safe, since internally it maintains +// some per-thread scratch space. + +#ifndef CERES_INTERNAL_PROGRAM_EVALUATOR_H_ +#define CERES_INTERNAL_PROGRAM_EVALUATOR_H_ + +#ifdef CERES_USE_OPENMP +#include <omp.h> +#endif + +#include "ceres/parameter_block.h" +#include "ceres/program.h" +#include "ceres/residual_block.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" + +namespace ceres { +namespace internal { + +template<typename EvaluatePreparer, typename JacobianWriter> +class ProgramEvaluator : public Evaluator { + public: + ProgramEvaluator(const Evaluator::Options &options, Program* program) + : options_(options), + program_(program), + jacobian_writer_(options, program), + evaluate_preparers_( + jacobian_writer_.CreateEvaluatePreparers(options.num_threads)) { +#ifndef CERES_USE_OPENMP + CHECK_EQ(1, options_.num_threads) + << "OpenMP support is not compiled into this binary; " + << "only options.num_threads=1 is supported."; +#endif + + BuildResidualLayout(*program, &residual_layout_); + evaluate_scratch_.reset(CreateEvaluatorScratch(*program, + options.num_threads)); + } + + // Implementation of Evaluator interface. + SparseMatrix* CreateJacobian() const { + return jacobian_writer_.CreateJacobian(); + } + + bool Evaluate(const double* state, + double* cost, + double* residuals, + SparseMatrix* jacobian) { + // The parameters are stateful, so set the state before evaluating. + if (!program_->StateVectorToParameterBlocks(state)) { + return false; + } + + if (jacobian) { + jacobian->SetZero(); + } + + // Each thread gets it's own cost and evaluate scratch space. + for (int i = 0; i < options_.num_threads; ++i) { + evaluate_scratch_[i].cost = 0.0; + } + + // This bool is used to disable the loop if an error is encountered + // without breaking out of it. The remaining loop iterations are still run, + // but with an empty body, and so will finish quickly. + bool abort = false; + int num_residual_blocks = program_->NumResidualBlocks(); +#pragma omp parallel for num_threads(options_.num_threads) + for (int i = 0; i < num_residual_blocks; ++i) { +// Disable the loop instead of breaking, as required by OpenMP. +#pragma omp flush(abort) + if (abort) { + continue; + } + +#ifdef CERES_USE_OPENMP + int thread_id = omp_get_thread_num(); +#else + int thread_id = 0; +#endif + EvaluatePreparer* preparer = &evaluate_preparers_[thread_id]; + EvaluateScratch* scratch = &evaluate_scratch_[thread_id]; + + // Prepare block residuals if requested. + const ResidualBlock* residual_block = program_->residual_blocks()[i]; + double* block_residuals = (residuals != NULL) + ? (residuals + residual_layout_[i]) + : NULL; + + // Prepare block jacobians if requested. + double** block_jacobians = NULL; + if (jacobian != NULL) { + preparer->Prepare(residual_block, + i, + jacobian, + scratch->jacobian_block_ptrs.get()); + block_jacobians = scratch->jacobian_block_ptrs.get(); + } + + // Evaluate the cost, residuals, and jacobians. + double block_cost; + if (!residual_block->Evaluate(&block_cost, + block_residuals, + block_jacobians, + scratch->scratch.get())) { + abort = true; +// This ensures that the OpenMP threads have a consistent view of 'abort'. Do +// the flush inside the failure case so that there is usually only one +// synchronization point per loop iteration instead of two. +#pragma omp flush(abort) + continue; + } + + scratch->cost += block_cost; + + if (jacobian != NULL) { + jacobian_writer_.Write(i, + residual_layout_[i], + block_jacobians, + jacobian); + } + } + + if (!abort) { + // Sum the cost from each thread. + (*cost) = 0.0; + for (int i = 0; i < options_.num_threads; ++i) { + (*cost) += evaluate_scratch_[i].cost; + } + } + return !abort; + } + + bool Plus(const double* state, + const double* delta, + double* state_plus_delta) const { + return program_->Plus(state, delta, state_plus_delta); + } + + int NumParameters() const { + return program_->NumParameters(); + } + int NumEffectiveParameters() const { + return program_->NumEffectiveParameters(); + } + + int NumResiduals() const { + return program_->NumResiduals(); + } + + private: + struct EvaluateScratch { + void Init(int max_parameters_per_residual_block, + int max_scratch_doubles_needed_for_evaluate) { + jacobian_block_ptrs.reset( + new double*[max_parameters_per_residual_block]); + scratch.reset(new double[max_scratch_doubles_needed_for_evaluate]); + } + + double cost; + scoped_array<double> scratch; + scoped_array<double*> jacobian_block_ptrs; + }; + + static void BuildResidualLayout(const Program& program, + vector<int>* residual_layout) { + const vector<ResidualBlock*>& residual_blocks = program.residual_blocks(); + residual_layout->resize(program.NumResidualBlocks()); + int residual_pos = 0; + for (int i = 0; i < residual_blocks.size(); ++i) { + const int num_residuals = residual_blocks[i]->NumResiduals(); + (*residual_layout)[i] = residual_pos; + residual_pos += num_residuals; + } + } + + // Create scratch space for each thread evaluating the program. + static EvaluateScratch* CreateEvaluatorScratch(const Program& program, + int num_threads) { + int max_parameters_per_residual_block = + program.MaxParametersPerResidualBlock(); + int max_scratch_doubles_needed_for_evaluate = + program.MaxScratchDoublesNeededForEvaluate(); + + EvaluateScratch* evaluate_scratch = new EvaluateScratch[num_threads]; + for (int i = 0; i < num_threads; i++) { + evaluate_scratch[i].Init(max_parameters_per_residual_block, + max_scratch_doubles_needed_for_evaluate); + } + return evaluate_scratch; + } + + Evaluator::Options options_; + Program* program_; + JacobianWriter jacobian_writer_; + scoped_array<EvaluatePreparer> evaluate_preparers_; + scoped_array<EvaluateScratch> evaluate_scratch_; + vector<int> residual_layout_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_PROGRAM_EVALUATOR_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/random.h b/extern/libmv/third_party/ceres/internal/ceres/random.h new file mode 100644 index 00000000000..769e0b4dd27 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/random.h @@ -0,0 +1,47 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#ifndef CERES_INTERNAL_RANDOM_H_ +#define CERES_INTERNAL_RANDOM_H_ + +namespace ceres { + +inline double RandDouble() { + double r = rand(); + return r / RAND_MAX; +} + +inline int Uniform(int n) { + return rand() % n; +} + +} // namespace ceres + +#endif // CERES_INTERNAL_RANDOM_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/residual_block.cc b/extern/libmv/third_party/ceres/internal/ceres/residual_block.cc new file mode 100644 index 00000000000..03867891dba --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/residual_block.cc @@ -0,0 +1,212 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/residual_block.h" + +#include <algorithm> +#include <cstddef> +#include <vector> + +#include "ceres/corrector.h" +#include "ceres/parameter_block.h" +#include "ceres/residual_block_utils.h" +#include "ceres/cost_function.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/fixed_array.h" +#include "ceres/local_parameterization.h" +#include "ceres/loss_function.h" + +namespace ceres { +namespace internal { + +ResidualBlock::ResidualBlock(const CostFunction* cost_function, + const LossFunction* loss_function, + const vector<ParameterBlock*>& parameter_blocks) + : cost_function_(cost_function), + loss_function_(loss_function), + parameter_blocks_( + new ParameterBlock* [ + cost_function->parameter_block_sizes().size()]) { + std::copy(parameter_blocks.begin(), + parameter_blocks.end(), + parameter_blocks_.get()); +} + +bool ResidualBlock::Evaluate(double* cost, + double* residuals, + double** jacobians, + double* scratch) const { + const int num_parameter_blocks = NumParameterBlocks(); + const int num_residuals = cost_function_->num_residuals(); + + // Collect the parameters from their blocks. This will rarely allocate, since + // residuals taking more than 8 parameter block arguments are rare. + FixedArray<const double*, 8> parameters(num_parameter_blocks); + for (int i = 0; i < num_parameter_blocks; ++i) { + parameters[i] = parameter_blocks_[i]->state(); + } + + // Put pointers into the scratch space into global_jacobians as appropriate. + FixedArray<double*, 8> global_jacobians(num_parameter_blocks); + if (jacobians != NULL) { + for (int i = 0; i < num_parameter_blocks; ++i) { + const ParameterBlock* parameter_block = parameter_blocks_[i]; + if (jacobians[i] != NULL && + parameter_block->LocalParameterizationJacobian() != NULL) { + global_jacobians[i] = scratch; + scratch += num_residuals * parameter_block->Size(); + } else { + global_jacobians[i] = jacobians[i]; + } + } + } + + // If the caller didn't request residuals, use the scratch space for them. + bool outputting_residuals = (residuals != NULL); + if (!outputting_residuals) { + residuals = scratch; + } + + // Invalidate the evaluation buffers so that we can check them after + // the CostFunction::Evaluate call, to see if all the return values + // that were required were written to and that they are finite. + double** eval_jacobians = (jacobians != NULL) ? global_jacobians.get() : NULL; + + InvalidateEvaluation(*this, cost, residuals, eval_jacobians); + + if (!cost_function_->Evaluate(parameters.get(), residuals, eval_jacobians) || + !IsEvaluationValid(*this, + parameters.get(), + cost, + residuals, + eval_jacobians)) { + string message = + "\n\n" + "Error in evaluating the ResidualBlock.\n\n" + "There are two possible reasons. Either the CostFunction did not evaluate and fill all \n" // NOLINT + "residual and jacobians that were requested or there was a non-finite value (nan/infinite)\n" // NOLINT + "generated during the or jacobian computation. \n\n" + + EvaluationToString(*this, + parameters.get(), + cost, + residuals, + eval_jacobians); + LOG(WARNING) << message; + return false; + } + + double squared_norm = VectorRef(residuals, num_residuals).squaredNorm(); + + // Update the jacobians with the local parameterizations. + if (jacobians != NULL) { + for (int i = 0; i < num_parameter_blocks; ++i) { + if (jacobians[i] != NULL) { + const ParameterBlock* parameter_block = parameter_blocks_[i]; + + // Apply local reparameterization to the jacobians. + if (parameter_block->LocalParameterizationJacobian() != NULL) { + ConstMatrixRef local_to_global( + parameter_block->LocalParameterizationJacobian(), + parameter_block->Size(), + parameter_block->LocalSize()); + MatrixRef global_jacobian(global_jacobians[i], + num_residuals, + parameter_block->Size()); + MatrixRef local_jacobian(jacobians[i], + num_residuals, + parameter_block->LocalSize()); + local_jacobian.noalias() = global_jacobian * local_to_global; + } + } + } + } + + if (loss_function_ == NULL) { + *cost = 0.5 * squared_norm; + return true; + } + + double rho[3]; + loss_function_->Evaluate(squared_norm, rho); + *cost = 0.5 * rho[0]; + + // No jacobians and not outputting residuals? All done. Doing an early exit + // here avoids constructing the "Corrector" object below in a common case. + if (jacobians == NULL && !outputting_residuals) { + return true; + } + + // Correct for the effects of the loss function. The jacobians need to be + // corrected before the residuals, since they use the uncorrected residuals. + Corrector correct(squared_norm, rho); + if (jacobians != NULL) { + for (int i = 0; i < num_parameter_blocks; ++i) { + if (jacobians[i] != NULL) { + const ParameterBlock* parameter_block = parameter_blocks_[i]; + + // Correct the jacobians for the loss function. + correct.CorrectJacobian(num_residuals, + parameter_block->LocalSize(), + residuals, + jacobians[i]); + } + } + } + + // Correct the residuals with the loss function. + if (outputting_residuals) { + correct.CorrectResiduals(num_residuals, residuals); + } + return true; +} + +int ResidualBlock::NumScratchDoublesForEvaluate() const { + // Compute the amount of scratch space needed to store the full-sized + // jacobians. For parameters that have no local parameterization no storage + // is needed and the passed-in jacobian array is used directly. Also include + // space to store the residuals, which is needed for cost-only evaluations. + // This is slightly pessimistic, since both won't be needed all the time, but + // the amount of excess should not cause problems for the caller. + int num_parameters = NumParameterBlocks(); + int scratch_doubles = 1; + for (int i = 0; i < num_parameters; ++i) { + const ParameterBlock* parameter_block = parameter_blocks_[i]; + if (!parameter_block->IsConstant() && + parameter_block->LocalParameterizationJacobian() != NULL) { + scratch_doubles += parameter_block->Size(); + } + } + scratch_doubles *= NumResiduals(); + return scratch_doubles; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/residual_block.h b/extern/libmv/third_party/ceres/internal/ceres/residual_block.h new file mode 100644 index 00000000000..e0a06e78958 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/residual_block.h @@ -0,0 +1,124 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// keir@google.com (Keir Mierle) +// +// Purpose : Class and struct definitions for parameter and residual blocks. + +#ifndef CERES_INTERNAL_RESIDUAL_BLOCK_H_ +#define CERES_INTERNAL_RESIDUAL_BLOCK_H_ + +#include <vector> + +#include "ceres/cost_function.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { + +class LossFunction; + +namespace internal { + +class ParameterBlock; + +// A term in the least squares problem. The mathematical form of each term in +// the overall least-squares cost function is: +// +// 1 +// --- loss_function( || cost_function(block1, block2, ...) ||^2 ), +// 2 +// +// Storing the cost function and the loss function separately permits optimizing +// the problem with standard non-linear least techniques, without requiring a +// more general non-linear solver. +// +// The residual block stores pointers to but does not own the cost functions, +// loss functions, and parameter blocks. +class ResidualBlock { + public: + ResidualBlock(const CostFunction* cost_function, + const LossFunction* loss_function, + const vector<ParameterBlock*>& parameter_blocks); + + // Evaluates the residual term, storing the scalar cost in *cost, the residual + // components in *residuals, and the jacobians between the parameters and + // residuals in jacobians[i], in row-major order. If residuals is NULL, the + // residuals are not computed. If jacobians is NULL, no jacobians are + // computed. If jacobians[i] is NULL, then the jacobian for that parameter is + // not computed. + // + // Evaluate needs scratch space which must be supplied by the caller via + // scratch. The array should have at least NumScratchDoublesForEvaluate() + // space available. + // + // The return value indicates the success or failure. If the function returns + // false, the caller should expect the the output memory locations to have + // been modified. + // + // The returned cost and jacobians have had robustification and local + // parameterizations applied already; for example, the jacobian for a + // 4-dimensional quaternion parameter using the "QuaternionParameterization" + // is num_residuals by 3 instead of num_residuals by 4. + bool Evaluate(double* cost, + double* residuals, + double** jacobians, + double* scratch) const; + + const CostFunction* cost_function() const { return cost_function_; } + const LossFunction* loss_function() const { return loss_function_; } + + // Access the parameter blocks for this residual. The array has size + // NumParameterBlocks(). + ParameterBlock* const* parameter_blocks() const { + return parameter_blocks_.get(); + } + + // Number of variable blocks that this residual term depends on. + int NumParameterBlocks() const { + return cost_function_->parameter_block_sizes().size(); + } + + // The size of the residual vector returned by this residual function. + int NumResiduals() const { return cost_function_->num_residuals(); } + + // The minimum amount of scratch space needed to pass to Evaluate(). + int NumScratchDoublesForEvaluate() const; + + private: + const CostFunction* cost_function_; + const LossFunction* loss_function_; + scoped_array<ParameterBlock*> parameter_blocks_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_RESIDUAL_BLOCK_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/residual_block_utils.cc b/extern/libmv/third_party/ceres/internal/ceres/residual_block_utils.cc new file mode 100644 index 00000000000..28e03130844 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/residual_block_utils.cc @@ -0,0 +1,185 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/residual_block_utils.h" + +#include <cmath> +#include <cstddef> +#include <limits> +#include <glog/logging.h> +#include "ceres/residual_block.h" +#include "ceres/parameter_block.h" +#include "ceres/stringprintf.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/port.h" + +#ifdef _MSC_VER +# define isfinite _finite +#endif + +namespace ceres { +namespace internal { + +// It is a near impossibility that user code generates this exact +// value in normal operation, thus we will use it to fill arrays +// before passing them to user code. If on return an element of the +// array still contains this value, we will assume that the user code +// did not write to that memory location. +static const double kImpossibleValue = 1e302; + +bool IsArrayValid(const int size, const double* x) { + if (x != NULL) { + for (int i = 0; i < size; ++i) { + if (!isfinite(x[i]) || (x[i] == kImpossibleValue)) { + return false; + } + } + } + return true; +} + +void InvalidateArray(const int size, double* x) { + if (x != NULL) { + for (int i = 0; i < size; ++i) { + x[i] = kImpossibleValue; + } + } +} + +void InvalidateEvaluation(const ResidualBlock& block, + double* cost, + double* residuals, + double** jacobians) { + const int num_parameter_blocks = block.NumParameterBlocks(); + const int num_residuals = block.NumResiduals(); + + InvalidateArray(1, cost); + InvalidateArray(num_residuals, residuals); + if (jacobians != NULL) { + for (int i = 0; i < num_parameter_blocks; ++i) { + const int parameter_block_size = block.parameter_blocks()[i]->Size(); + InvalidateArray(num_residuals * parameter_block_size, jacobians[i]); + } + } +} + +// Utility routine to print an array of doubles to a string. If the +// array pointer is NULL, it is treated as an array of zeros. +void AppendArrayToString(const int size, const double* x, string* result) { + for (int i = 0; i < size; ++i) { + if (x == NULL) { + StringAppendF(result, "Not Computed "); + } else { + if (x[i] == kImpossibleValue) { + StringAppendF(result, "Uninitialized "); + } else { + StringAppendF(result, "%12g ", x[i]); + } + } + } +} + +string EvaluationToString(const ResidualBlock& block, + double const* const* parameters, + double* cost, + double* residuals, + double** jacobians) { + CHECK_NOTNULL(cost); + CHECK_NOTNULL(residuals); + + const int num_parameter_blocks = block.NumParameterBlocks(); + const int num_residuals = block.NumResiduals(); + string result = ""; + + StringAppendF(&result, + "Residual Block size: %d parameter blocks x %d residuals\n\n", + num_parameter_blocks, num_residuals); + result += + "For each parameter block, the value of the parameters are printed in the first column \n" // NOLINT + "and the value of the jacobian under the corresponding residual. If a ParameterBlock was \n" // NOLINT + "held constant then the corresponding jacobian is printed as 'Not Computed'. If an entry \n" // NOLINT + "of the Jacobian/residual array was requested but was not written to by user code, it is \n" // NOLINT + "indicated by 'Uninitialized'. This is an error. Residuals or Jacobian values evaluating \n" // NOLINT + "to Inf or NaN is also an error. \n\n"; // NOLINT + + string space = "Residuals: "; + result += space; + AppendArrayToString(num_residuals, residuals, &result); + StringAppendF(&result, "\n\n"); + + for (int i = 0; i < num_parameter_blocks; ++i) { + const int parameter_block_size = block.parameter_blocks()[i]->Size(); + StringAppendF( + &result, "Parameter Block %d, size: %d\n", i, parameter_block_size); + StringAppendF(&result, "\n"); + for (int j = 0; j < parameter_block_size; ++j) { + AppendArrayToString(1, parameters[i] + j, &result); + StringAppendF(&result, "| "); + for (int k = 0; k < num_residuals; ++k) { + AppendArrayToString(1, + (jacobians != NULL && jacobians[i] != NULL) + ? jacobians[i] + k * parameter_block_size + j + : NULL, + &result); + } + StringAppendF(&result, "\n"); + } + StringAppendF(&result, "\n"); + } + StringAppendF(&result, "\n"); + return result; +} + +bool IsEvaluationValid(const ResidualBlock& block, + double const* const* parameters, + double* cost, + double* residuals, + double** jacobians) { + const int num_parameter_blocks = block.NumParameterBlocks(); + const int num_residuals = block.NumResiduals(); + + if (!IsArrayValid(num_residuals, residuals)) { + return false; + } + + if (jacobians != NULL) { + for (int i = 0; i < num_parameter_blocks; ++i) { + const int parameter_block_size = block.parameter_blocks()[i]->Size(); + if (!IsArrayValid(num_residuals * parameter_block_size, jacobians[i])) { + return false; + } + } + } + + return true; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/residual_block_utils.h b/extern/libmv/third_party/ceres/internal/ceres/residual_block_utils.h new file mode 100644 index 00000000000..228867cc60c --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/residual_block_utils.h @@ -0,0 +1,89 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Utility routines for ResidualBlock evaluation. +// +// These are useful for detecting two common class of errors. +// +// 1. Uninitialized memory - where the user for some reason did not +// compute part of a cost/residual/jacobian. +// +// 2. Numerical failure while computing the cost/residual/jacobian, +// e.g. NaN, infinities etc. This is particularly useful since the +// automatic differentiation code does computations that are not +// evident to the user and can silently generate hard to debug errors. + +#ifndef CERES_INTERNAL_RESIDUAL_BLOCK_UTILS_H_ +#define CERES_INTERNAL_RESIDUAL_BLOCK_UTILS_H_ + +#include <string> +#include "ceres/internal/port.h" + +namespace ceres { +namespace internal { + +class ResidualBlock; + +// Fill the array x with an impossible value that the user code is +// never expected to compute. +void InvalidateArray(int size, double* x); + +// Check if all the entries of the array x are valid, i.e. all the +// values in the array should be finite and none of them should be +// equal to the "impossible" value used by InvalidateArray. +bool IsArrayValid(int size, const double* x); + +// Invalidate cost, resdual and jacobian arrays (if not NULL). +void InvalidateEvaluation(const ResidualBlock& block, + double* cost, + double* residuals, + double** jacobians); + +// Check if any of the arrays cost, residuals or jacobians contains an +// NaN, return true if it does. +bool IsEvaluationValid(const ResidualBlock& block, + double const* const* parameters, + double* cost, + double* residuals, + double** jacobians); + +// Create a string representation of the Residual block containing the +// value of the parameters, residuals and jacobians if present. +// Useful for debugging output. +string EvaluationToString(const ResidualBlock& block, + double const* const* parameters, + double* cost, + double* residuals, + double** jacobians); + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_RESIDUAL_BLOCK_UTILS_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/runtime_numeric_diff_cost_function.cc b/extern/libmv/third_party/ceres/internal/ceres/runtime_numeric_diff_cost_function.cc new file mode 100644 index 00000000000..ac6d8aa279a --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/runtime_numeric_diff_cost_function.cc @@ -0,0 +1,218 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// Based on the templated version in public/numeric_diff_cost_function.h. + +#include "ceres/runtime_numeric_diff_cost_function.h" + +#include <algorithm> +#include <numeric> +#include <vector> + +#include <glog/logging.h> +#include "Eigen/Dense" +#include "ceres/cost_function.h" +#include "ceres/internal/scoped_ptr.h" + +namespace ceres { +namespace internal { +namespace { + +bool EvaluateJacobianForParameterBlock(const CostFunction* function, + int parameter_block_size, + int parameter_block, + RuntimeNumericDiffMethod method, + double relative_step_size, + double const* residuals_at_eval_point, + double** parameters, + double** jacobians) { + using Eigen::Map; + using Eigen::Matrix; + using Eigen::Dynamic; + using Eigen::RowMajor; + + typedef Matrix<double, Dynamic, 1> ResidualVector; + typedef Matrix<double, Dynamic, 1> ParameterVector; + typedef Matrix<double, Dynamic, Dynamic, RowMajor> JacobianMatrix; + + int num_residuals = function->num_residuals(); + + Map<JacobianMatrix> parameter_jacobian(jacobians[parameter_block], + num_residuals, + parameter_block_size); + + // Mutate one element at a time and then restore. + Map<ParameterVector> x_plus_delta(parameters[parameter_block], + parameter_block_size); + ParameterVector x(x_plus_delta); + ParameterVector step_size = x.array().abs() * relative_step_size; + + // To handle cases where a paremeter is exactly zero, instead use the mean + // step_size for the other dimensions. + double fallback_step_size = step_size.sum() / step_size.rows(); + if (fallback_step_size == 0.0) { + // If all the parameters are zero, there's no good answer. Use the given + // relative step_size as absolute step_size and hope for the best. + fallback_step_size = relative_step_size; + } + + // For each parameter in the parameter block, use finite differences to + // compute the derivative for that parameter. + for (int j = 0; j < parameter_block_size; ++j) { + if (step_size(j) == 0.0) { + // The parameter is exactly zero, so compromise and use the mean step_size + // from the other parameters. This can break in many cases, but it's hard + // to pick a good number without problem specific knowledge. + step_size(j) = fallback_step_size; + } + x_plus_delta(j) = x(j) + step_size(j); + + ResidualVector residuals(num_residuals); + if (!function->Evaluate(parameters, &residuals[0], NULL)) { + // Something went wrong; bail. + return false; + } + + // Compute this column of the jacobian in 3 steps: + // 1. Store residuals for the forward part. + // 2. Subtract residuals for the backward (or 0) part. + // 3. Divide out the run. + parameter_jacobian.col(j) = residuals; + + double one_over_h = 1 / step_size(j); + if (method == CENTRAL) { + // Compute the function on the other side of x(j). + x_plus_delta(j) = x(j) - step_size(j); + + if (!function->Evaluate(parameters, &residuals[0], NULL)) { + // Something went wrong; bail. + return false; + } + parameter_jacobian.col(j) -= residuals; + one_over_h /= 2; + } else { + // Forward difference only; reuse existing residuals evaluation. + parameter_jacobian.col(j) -= + Map<const ResidualVector>(residuals_at_eval_point, num_residuals); + } + x_plus_delta(j) = x(j); // Restore x_plus_delta. + + // Divide out the run to get slope. + parameter_jacobian.col(j) *= one_over_h; + } + return true; +} + +class RuntimeNumericDiffCostFunction : public CostFunction { + public: + RuntimeNumericDiffCostFunction(const CostFunction* function, + RuntimeNumericDiffMethod method, + double relative_step_size) + : function_(function), + method_(method), + relative_step_size_(relative_step_size) { + *mutable_parameter_block_sizes() = function->parameter_block_sizes(); + set_num_residuals(function->num_residuals()); + } + + virtual ~RuntimeNumericDiffCostFunction() { } + + virtual bool Evaluate(double const* const* parameters, + double* residuals, + double** jacobians) const { + // Get the function value (residuals) at the the point to evaluate. + bool success = function_->Evaluate(parameters, residuals, NULL); + if (!success) { + // Something went wrong; ignore the jacobian. + return false; + } + if (!jacobians) { + // Nothing to do; just forward. + return true; + } + + const vector<int16>& block_sizes = function_->parameter_block_sizes(); + CHECK(!block_sizes.empty()); + + // Create local space for a copy of the parameters which will get mutated. + int parameters_size = accumulate(block_sizes.begin(), block_sizes.end(), 0); + vector<double> parameters_copy(parameters_size); + vector<double*> parameters_references_copy(block_sizes.size()); + parameters_references_copy[0] = ¶meters_copy[0]; + for (int block = 1; block < block_sizes.size(); ++block) { + parameters_references_copy[block] = parameters_references_copy[block - 1] + + block_sizes[block - 1]; + } + + // Copy the parameters into the local temp space. + for (int block = 0; block < block_sizes.size(); ++block) { + memcpy(parameters_references_copy[block], + parameters[block], + block_sizes[block] * sizeof(*parameters[block])); + } + + for (int block = 0; block < block_sizes.size(); ++block) { + if (!jacobians[block]) { + // No jacobian requested for this parameter / residual pair. + continue; + } + if (!EvaluateJacobianForParameterBlock(function_, + block_sizes[block], + block, + method_, + relative_step_size_, + residuals, + ¶meters_references_copy[0], + jacobians)) { + return false; + } + } + return true; + } + + private: + const CostFunction* function_; + RuntimeNumericDiffMethod method_; + double relative_step_size_; +}; + +} // namespace + +CostFunction* CreateRuntimeNumericDiffCostFunction( + const CostFunction* cost_function, + RuntimeNumericDiffMethod method, + double relative_step_size) { + return new RuntimeNumericDiffCostFunction(cost_function, + method, + relative_step_size); +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/runtime_numeric_diff_cost_function.h b/extern/libmv/third_party/ceres/internal/ceres/runtime_numeric_diff_cost_function.h new file mode 100644 index 00000000000..01b57f92ef3 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/runtime_numeric_diff_cost_function.h @@ -0,0 +1,87 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// Create CostFunctions as needed by the least squares framework with jacobians +// computed via numeric differentiation. +// +// To get a numerically differentiated cost function, define a subclass of +// CostFunction such that the Evaluate() function ignores the jacobian +// parameter. The numeric differentiation wrapper will fill in the jacobian +// parameter if nececssary by repeatedly calling the Evaluate() function with +// small changes to the appropriate parameters, and computing the slope. This +// implementation is not templated (hence the "Runtime" prefix), which is a bit +// slower than but is more convenient than the templated version in +// numeric_diff_cost_function.h +// +// The numerically differentiated version of a cost function for a cost function +// can be constructed as follows: +// +// CostFunction* cost_function = +// CreateRuntimeNumericDiffCostFunction(new MyCostFunction(...), +// CENTRAL, +// TAKE_OWNERSHIP); +// +// The central difference method is considerably more accurate; consider using +// to start and only after that works, trying forward difference. +// +// TODO(keir): Characterize accuracy; mention pitfalls; provide alternatives. + +#ifndef CERES_INTERNAL_RUNTIME_NUMERIC_DIFF_COST_FUNCTION_H_ +#define CERES_INTERNAL_RUNTIME_NUMERIC_DIFF_COST_FUNCTION_H_ + +#include "ceres/cost_function.h" + +namespace ceres { +namespace internal { + +enum RuntimeNumericDiffMethod { + CENTRAL, + FORWARD, +}; + +// Create a cost function that evaluates the derivative with finite differences. +// The base cost_function's implementation of Evaluate() only needs to fill in +// the "residuals" argument and not the "jacobians". Any data written to the +// jacobians by the base cost_function is overwritten. +// +// Forward difference or central difference is selected with CENTRAL or FORWARD. +// The relative eps, which determines the step size for forward and central +// differencing, is set with relative eps. Caller owns the resulting cost +// function, and the resulting cost function does not own the base cost +// function. +CostFunction *CreateRuntimeNumericDiffCostFunction( + const CostFunction *cost_function, + RuntimeNumericDiffMethod method, + double relative_eps); + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_RUNTIME_NUMERIC_DIFF_COST_FUNCTION_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/schur_complement_solver.cc b/extern/libmv/third_party/ceres/internal/ceres/schur_complement_solver.cc new file mode 100644 index 00000000000..2bc8cdd6bec --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/schur_complement_solver.cc @@ -0,0 +1,285 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include <algorithm> +#include <ctime> +#include <set> +#include <vector> +#include "Eigen/Dense" +#include "ceres/block_random_access_dense_matrix.h" +#include "ceres/block_random_access_matrix.h" +#include "ceres/block_random_access_sparse_matrix.h" +#include "ceres/block_sparse_matrix.h" +#include "ceres/block_structure.h" +#include "ceres/detect_structure.h" +#include "ceres/linear_solver.h" +#include "ceres/schur_complement_solver.h" +#include "ceres/suitesparse.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +LinearSolver::Summary SchurComplementSolver::SolveImpl( + BlockSparseMatrixBase* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x) { + const time_t start_time = time(NULL); + if (!options_.constant_sparsity || (eliminator_.get() == NULL)) { + InitStorage(A->block_structure()); + DetectStructure(*A->block_structure(), + options_.num_eliminate_blocks, + &options_.row_block_size, + &options_.e_block_size, + &options_.f_block_size); + eliminator_.reset(CHECK_NOTNULL(SchurEliminatorBase::Create(options_))); + eliminator_->Init(options_.num_eliminate_blocks, A->block_structure()); + }; + const time_t init_time = time(NULL); + fill(x, x + A->num_cols(), 0.0); + + LinearSolver::Summary summary; + summary.num_iterations = 1; + summary.termination_type = FAILURE; + eliminator_->Eliminate(A, b, per_solve_options.D, lhs_.get(), rhs_.get()); + const time_t eliminate_time = time(NULL); + + double* reduced_solution = x + A->num_cols() - lhs_->num_cols(); + const bool status = SolveReducedLinearSystem(reduced_solution); + const time_t solve_time = time(NULL); + + if (!status) { + return summary; + } + + eliminator_->BackSubstitute(A, b, per_solve_options.D, reduced_solution, x); + const time_t backsubstitute_time = time(NULL); + summary.termination_type = TOLERANCE; + + VLOG(2) << "time (sec) total: " << backsubstitute_time - start_time + << " init: " << init_time - start_time + << " eliminate: " << eliminate_time - init_time + << " solve: " << solve_time - eliminate_time + << " backsubstitute: " << backsubstitute_time - solve_time; + return summary; +} + +// Initialize a BlockRandomAccessDenseMatrix to store the Schur +// complement. +void DenseSchurComplementSolver::InitStorage( + const CompressedRowBlockStructure* bs) { + const int num_eliminate_blocks = options().num_eliminate_blocks; + const int num_col_blocks = bs->cols.size(); + + vector<int> blocks(num_col_blocks - num_eliminate_blocks, 0); + for (int i = num_eliminate_blocks, j = 0; + i < num_col_blocks; + ++i, ++j) { + blocks[j] = bs->cols[i].size; + } + + set_lhs(new BlockRandomAccessDenseMatrix(blocks)); + set_rhs(new double[lhs()->num_rows()]); +} + +// Solve the system Sx = r, assuming that the matrix S is stored in a +// BlockRandomAccessDenseMatrix. The linear system is solved using +// Eigen's Cholesky factorization. +bool DenseSchurComplementSolver::SolveReducedLinearSystem(double* solution) { + const BlockRandomAccessDenseMatrix* m = + down_cast<const BlockRandomAccessDenseMatrix*>(lhs()); + const int num_rows = m->num_rows(); + + // The case where there are no f blocks, and the system is block + // diagonal. + if (num_rows == 0) { + return true; + } + + // TODO(sameeragarwal): Add proper error handling; this completely ignores + // the quality of the solution to the solve. + VectorRef(solution, num_rows) = + ConstMatrixRef(m->values(), num_rows, num_rows) + .selfadjointView<Eigen::Upper>() + .ldlt() + .solve(ConstVectorRef(rhs(), num_rows)); + + return true; +} + +#ifndef CERES_NO_SUITESPARSE +SparseSchurComplementSolver::SparseSchurComplementSolver( + const LinearSolver::Options& options) + : SchurComplementSolver(options), + symbolic_factor_(NULL) { +} + +SparseSchurComplementSolver::~SparseSchurComplementSolver() { + if (symbolic_factor_ != NULL) { + ss_.Free(symbolic_factor_); + symbolic_factor_ = NULL; + } +} + +// Determine the non-zero blocks in the Schur Complement matrix, and +// initialize a BlockRandomAccessSparseMatrix object. +void SparseSchurComplementSolver::InitStorage( + const CompressedRowBlockStructure* bs) { + const int num_eliminate_blocks = options().num_eliminate_blocks; + const int num_col_blocks = bs->cols.size(); + const int num_row_blocks = bs->rows.size(); + + vector<int> blocks(num_col_blocks - num_eliminate_blocks, 0); + for (int i = num_eliminate_blocks; i < num_col_blocks; ++i) { + blocks[i - num_eliminate_blocks] = bs->cols[i].size; + } + + set<pair<int, int> > block_pairs; + for (int i = 0; i < blocks.size(); ++i) { + block_pairs.insert(make_pair(i, i)); + } + + int r = 0; + while (r < num_row_blocks) { + int e_block_id = bs->rows[r].cells.front().block_id; + if (e_block_id >= num_eliminate_blocks) { + break; + } + vector<int> f_blocks; + + // Add to the chunk until the first block in the row is + // different than the one in the first row for the chunk. + for (; r < num_row_blocks; ++r) { + const CompressedRow& row = bs->rows[r]; + if (row.cells.front().block_id != e_block_id) { + break; + } + + // Iterate over the blocks in the row, ignoring the first + // block since it is the one to be eliminated. + for (int c = 1; c < row.cells.size(); ++c) { + const Cell& cell = row.cells[c]; + f_blocks.push_back(cell.block_id - num_eliminate_blocks); + } + } + + sort(f_blocks.begin(), f_blocks.end()); + f_blocks.erase(unique(f_blocks.begin(), f_blocks.end()), f_blocks.end()); + for (int i = 0; i < f_blocks.size(); ++i) { + for (int j = i + 1; j < f_blocks.size(); ++j) { + block_pairs.insert(make_pair(f_blocks[i], f_blocks[j])); + } + } + } + + // Remaing rows do not contribute to the chunks and directly go + // into the schur complement via an outer product. + for (; r < num_row_blocks; ++r) { + const CompressedRow& row = bs->rows[r]; + CHECK_GE(row.cells.front().block_id, num_eliminate_blocks); + for (int i = 0; i < row.cells.size(); ++i) { + int r_block1_id = row.cells[i].block_id - num_eliminate_blocks; + for (int j = 0; j < row.cells.size(); ++j) { + int r_block2_id = row.cells[j].block_id - num_eliminate_blocks; + if (r_block1_id <= r_block2_id) { + block_pairs.insert(make_pair(r_block1_id, r_block2_id)); + } + } + } + } + + set_lhs(new BlockRandomAccessSparseMatrix(blocks, block_pairs)); + set_rhs(new double[lhs()->num_rows()]); +} + +// Solve the system Sx = r, assuming that the matrix S is stored in a +// BlockRandomAccessSparseMatrix. The linear system is solved using +// CHOLMOD's sparse cholesky factorization routines. +bool SparseSchurComplementSolver::SolveReducedLinearSystem(double* solution) { + // Extract the TripletSparseMatrix that is used for actually storing S. + TripletSparseMatrix* tsm = + const_cast<TripletSparseMatrix*>( + down_cast<const BlockRandomAccessSparseMatrix*>(lhs())->matrix()); + + const int num_rows = tsm->num_rows(); + + // The case where there are no f blocks, and the system is block + // diagonal. + if (num_rows == 0) { + return true; + } + + cholmod_sparse* cholmod_lhs = ss_.CreateSparseMatrix(tsm); + // The matrix is symmetric, and the upper triangular part of the + // matrix contains the values. + cholmod_lhs->stype = 1; + + cholmod_dense* cholmod_rhs = + ss_.CreateDenseVector(const_cast<double*>(rhs()), num_rows, num_rows); + + // Symbolic factorization is computed if we don't already have one handy. + if (symbolic_factor_ == NULL) { + symbolic_factor_ = ss_.AnalyzeCholesky(cholmod_lhs); + } + + cholmod_dense* cholmod_solution = + ss_.SolveCholesky(cholmod_lhs, symbolic_factor_, cholmod_rhs); + + ss_.Free(cholmod_lhs); + cholmod_lhs = NULL; + ss_.Free(cholmod_rhs); + cholmod_rhs = NULL; + + // If sparsity is not constant across calls, then reset the symbolic + // factorization. + if (!options().constant_sparsity) { + ss_.Free(symbolic_factor_); + symbolic_factor_ = NULL; + } + + if (cholmod_solution == NULL) { + LOG(ERROR) << "CHOLMOD solve failed."; + return false; + } + + VectorRef(solution, num_rows) + = VectorRef(static_cast<double*>(cholmod_solution->x), num_rows); + ss_.Free(cholmod_solution); + return true; +} +#endif // CERES_NO_SUITESPARSE + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/schur_complement_solver.h b/extern/libmv/third_party/ceres/internal/ceres/schur_complement_solver.h new file mode 100644 index 00000000000..039bc09e3ce --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/schur_complement_solver.h @@ -0,0 +1,182 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_SCHUR_COMPLEMENT_SOLVER_H_ +#define CERES_INTERNAL_SCHUR_COMPLEMENT_SOLVER_H_ + +#include "ceres/block_random_access_matrix.h" +#include "ceres/block_sparse_matrix.h" +#include "ceres/block_structure.h" +#include "ceres/linear_solver.h" +#include "ceres/schur_eliminator.h" +#include "ceres/suitesparse.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +class BlockSparseMatrixBase; + +// Base class for Schur complement based linear least squares +// solvers. It assumes that the input linear system Ax = b can be +// partitioned into +// +// E y + F z = b +// +// Where x = [y;z] is a partition of the variables. The paritioning +// of the variables is such that, E'E is a block diagonal +// matrix. Further, the rows of A are ordered so that for every +// variable block in y, all the rows containing that variable block +// occur as a vertically contiguous block. i.e the matrix A looks like +// +// E F +// A = [ y1 0 0 0 | z1 0 0 0 z5] +// [ y1 0 0 0 | z1 z2 0 0 0] +// [ 0 y2 0 0 | 0 0 z3 0 0] +// [ 0 0 y3 0 | z1 z2 z3 z4 z5] +// [ 0 0 y3 0 | z1 0 0 0 z5] +// [ 0 0 0 y4 | 0 0 0 0 z5] +// [ 0 0 0 y4 | 0 z2 0 0 0] +// [ 0 0 0 y4 | 0 0 0 0 0] +// [ 0 0 0 0 | z1 0 0 0 0] +// [ 0 0 0 0 | 0 0 z3 z4 z5] +// +// This structure should be reflected in the corresponding +// CompressedRowBlockStructure object associated with A. The linear +// system Ax = b should either be well posed or the array D below +// should be non-null and the diagonal matrix corresponding to it +// should be non-singular. +// +// SchurComplementSolver has two sub-classes. +// +// DenseSchurComplementSolver: For problems where the Schur complement +// matrix is small and dense, or if CHOLMOD/SuiteSparse is not +// installed. For structure from motion problems, this is solver can +// be used for problems with upto a few hundred cameras. +// +// SparseSchurComplementSolver: For problems where the Schur +// complement matrix is large and sparse. It requires that +// CHOLMOD/SuiteSparse be installed, as it uses CHOLMOD to find a +// sparse Cholesky factorization of the Schur complement. This solver +// can be used for solving structure from motion problems with tens of +// thousands of cameras, though depending on the exact sparsity +// structure, it maybe better to use an iterative solver. +// +// The two solvers can be instantiated by calling +// LinearSolver::CreateLinearSolver with LinearSolver::Options::type +// set to DENSE_SCHUR and SPARSE_SCHUR +// respectively. LinearSolver::Options::num_eliminate_blocks should be +// at least 1. +class SchurComplementSolver : public BlockSparseMatrixBaseSolver { + public: + explicit SchurComplementSolver(const LinearSolver::Options& options) + : options_(options) { + CHECK_GT(options.num_eliminate_blocks, 0); + } + + // LinearSolver methods + virtual ~SchurComplementSolver() {} + virtual LinearSolver::Summary SolveImpl( + BlockSparseMatrixBase* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double* x); + + protected: + const LinearSolver::Options& options() const { return options_; } + + const BlockRandomAccessMatrix* lhs() const { return lhs_.get(); } + void set_lhs(BlockRandomAccessMatrix* lhs) { lhs_.reset(lhs); } + const double* rhs() const { return rhs_.get(); } + void set_rhs(double* rhs) { rhs_.reset(rhs); } + + private: + virtual void InitStorage(const CompressedRowBlockStructure* bs) = 0; + virtual bool SolveReducedLinearSystem(double* solution) = 0; + + LinearSolver::Options options_; + + scoped_ptr<SchurEliminatorBase> eliminator_; + scoped_ptr<BlockRandomAccessMatrix> lhs_; + scoped_array<double> rhs_; + + DISALLOW_COPY_AND_ASSIGN(SchurComplementSolver); +}; + +// Dense Cholesky factorization based solver. +class DenseSchurComplementSolver : public SchurComplementSolver { + public: + explicit DenseSchurComplementSolver(const LinearSolver::Options& options) + : SchurComplementSolver(options) {} + virtual ~DenseSchurComplementSolver() {} + + private: + virtual void InitStorage(const CompressedRowBlockStructure* bs); + virtual bool SolveReducedLinearSystem(double* solution); + + DISALLOW_COPY_AND_ASSIGN(DenseSchurComplementSolver); +}; + +#ifndef CERES_NO_SUITESPARSE +// Sparse Cholesky factorization based solver. +class SparseSchurComplementSolver : public SchurComplementSolver { + public: + explicit SparseSchurComplementSolver(const LinearSolver::Options& options); + virtual ~SparseSchurComplementSolver(); + + private: + virtual void InitStorage(const CompressedRowBlockStructure* bs); + virtual bool SolveReducedLinearSystem(double* solution); + + + SuiteSparse ss_; + // Symbolic factorization of the reduced linear system. Precomputed + // once and reused if constant_sparsity_ is true. + cholmod_factor* symbolic_factor_; + DISALLOW_COPY_AND_ASSIGN(SparseSchurComplementSolver); +}; +#else // CERES_NO_SUITESPARSE +class SparseSchurComplementSolver : public SchurComplementSolver { + public: + explicit SparseSchurComplementSolver(const LinearSolver::Options& options) + : SchurComplementSolver(options) { + LOG(FATAL) << "SPARSE_SCHUR is not available. Please " + "build Ceres with SuiteSparse."; + } + + virtual ~SparseSchurComplementSolver() {} +}; +#endif // CERES_NO_SUITESPARSE + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_SCHUR_COMPLEMENT_SOLVER_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/schur_eliminator.cc b/extern/libmv/third_party/ceres/internal/ceres/schur_eliminator.cc new file mode 100644 index 00000000000..44f5be3b4e9 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/schur_eliminator.cc @@ -0,0 +1,141 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// ======================================== +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +// THIS FILE IS AUTOGENERATED. DO NOT EDIT. +//========================================= +// +// This file is generated using generate_template_specializations.py. +// Editing it manually is not recommended. + +#include "ceres/linear_solver.h" +#include "ceres/schur_eliminator.h" +#include "ceres/internal/eigen.h" + +namespace ceres { +namespace internal { + +SchurEliminatorBase* +SchurEliminatorBase::Create(const LinearSolver::Options& options) { +#ifndef CERES_RESTRICT_SCHUR_SPECIALIZATION + if ((options.row_block_size == 2) && + (options.e_block_size == 2) && + (options.f_block_size == 2)) { + return new SchurEliminator<2, 2, 2>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 2) && + (options.f_block_size == 3)) { + return new SchurEliminator<2, 2, 3>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 2) && + (options.f_block_size == 4)) { + return new SchurEliminator<2, 2, 4>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 2) && + (options.f_block_size == Dynamic)) { + return new SchurEliminator<2, 2, Dynamic>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 3) && + (options.f_block_size == 3)) { + return new SchurEliminator<2, 3, 3>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 3) && + (options.f_block_size == 4)) { + return new SchurEliminator<2, 3, 4>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 3) && + (options.f_block_size == 9)) { + return new SchurEliminator<2, 3, 9>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 3) && + (options.f_block_size == Dynamic)) { + return new SchurEliminator<2, 3, Dynamic>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 4) && + (options.f_block_size == 3)) { + return new SchurEliminator<2, 4, 3>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 4) && + (options.f_block_size == 4)) { + return new SchurEliminator<2, 4, 4>(options); + } + if ((options.row_block_size == 2) && + (options.e_block_size == 4) && + (options.f_block_size == Dynamic)) { + return new SchurEliminator<2, 4, Dynamic>(options); + } + if ((options.row_block_size == 4) && + (options.e_block_size == 4) && + (options.f_block_size == 2)) { + return new SchurEliminator<4, 4, 2>(options); + } + if ((options.row_block_size == 4) && + (options.e_block_size == 4) && + (options.f_block_size == 3)) { + return new SchurEliminator<4, 4, 3>(options); + } + if ((options.row_block_size == 4) && + (options.e_block_size == 4) && + (options.f_block_size == 4)) { + return new SchurEliminator<4, 4, 4>(options); + } + if ((options.row_block_size == 4) && + (options.e_block_size == 4) && + (options.f_block_size == Dynamic)) { + return new SchurEliminator<4, 4, Dynamic>(options); + } + if ((options.row_block_size == Dynamic) && + (options.e_block_size == Dynamic) && + (options.f_block_size == Dynamic)) { + return new SchurEliminator<Dynamic, Dynamic, Dynamic>(options); + } + +#endif + VLOG(1) << "Template specializations not found for <" + << options.row_block_size << "," + << options.e_block_size << "," + << options.f_block_size << ">"; + return new SchurEliminator<Dynamic, Dynamic, Dynamic>(options); +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/schur_eliminator.h b/extern/libmv/third_party/ceres/internal/ceres/schur_eliminator.h new file mode 100644 index 00000000000..c24fe435f54 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/schur_eliminator.h @@ -0,0 +1,339 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_SCHUR_ELIMINATOR_H_ +#define CERES_INTERNAL_SCHUR_ELIMINATOR_H_ + +#include <map> +#include <vector> +#include "ceres/mutex.h" +#include "ceres/block_random_access_matrix.h" +#include "ceres/block_sparse_matrix.h" +#include "ceres/block_structure.h" +#include "ceres/linear_solver.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" + +namespace ceres { +namespace internal { + +// Classes implementing the SchurEliminatorBase interface implement +// variable elimination for linear least squares problems. Assuming +// that the input linear system Ax = b can be partitioned into +// +// E y + F z = b +// +// Where x = [y;z] is a partition of the variables. The paritioning +// of the variables is such that, E'E is a block diagonal matrix. Or +// in other words, the parameter blocks in E form an independent set +// of the of the graph implied by the block matrix A'A. Then, this +// class provides the functionality to compute the Schur complement +// system +// +// S z = r +// +// where +// +// S = F'F - F'E (E'E)^{-1} E'F and r = F'b - F'E(E'E)^(-1) E'b +// +// This is the Eliminate operation, i.e., construct the linear system +// obtained by eliminating the variables in E. +// +// The eliminator also provides the reverse functionality, i.e. given +// values for z it can back substitute for the values of y, by solving the +// linear system +// +// Ey = b - F z +// +// which is done by observing that +// +// y = (E'E)^(-1) [E'b - E'F z] +// +// The eliminator has a number of requirements. +// +// The rows of A are ordered so that for every variable block in y, +// all the rows containing that variable block occur as a vertically +// contiguous block. i.e the matrix A looks like +// +// E F chunk +// A = [ y1 0 0 0 | z1 0 0 0 z5] 1 +// [ y1 0 0 0 | z1 z2 0 0 0] 1 +// [ 0 y2 0 0 | 0 0 z3 0 0] 2 +// [ 0 0 y3 0 | z1 z2 z3 z4 z5] 3 +// [ 0 0 y3 0 | z1 0 0 0 z5] 3 +// [ 0 0 0 y4 | 0 0 0 0 z5] 4 +// [ 0 0 0 y4 | 0 z2 0 0 0] 4 +// [ 0 0 0 y4 | 0 0 0 0 0] 4 +// [ 0 0 0 0 | z1 0 0 0 0] non chunk blocks +// [ 0 0 0 0 | 0 0 z3 z4 z5] non chunk blocks +// +// This structure should be reflected in the corresponding +// CompressedRowBlockStructure object associated with A. The linear +// system Ax = b should either be well posed or the array D below +// should be non-null and the diagonal matrix corresponding to it +// should be non-singular. For simplicity of exposition only the case +// with a null D is described. +// +// The usual way to do the elimination is as follows. Starting with +// +// E y + F z = b +// +// we can form the normal equations, +// +// E'E y + E'F z = E'b +// F'E y + F'F z = F'b +// +// multiplying both sides of the first equation by (E'E)^(-1) and then +// by F'E we get +// +// F'E y + F'E (E'E)^(-1) E'F z = F'E (E'E)^(-1) E'b +// F'E y + F'F z = F'b +// +// now subtracting the two equations we get +// +// [FF' - F'E (E'E)^(-1) E'F] z = F'b - F'E(E'E)^(-1) E'b +// +// Instead of forming the normal equations and operating on them as +// general sparse matrices, the algorithm here deals with one +// parameter block in y at a time. The rows corresponding to a single +// parameter block yi are known as a chunk, and the algorithm operates +// on one chunk at a time. The mathematics remains the same since the +// reduced linear system can be shown to be the sum of the reduced +// linear systems for each chunk. This can be seen by observing two +// things. +// +// 1. E'E is a block diagonal matrix. +// +// 2. When E'F is computed, only the terms within a single chunk +// interact, i.e for y1 column blocks when transposed and multiplied +// with F, the only non-zero contribution comes from the blocks in +// chunk1. +// +// Thus, the reduced linear system +// +// FF' - F'E (E'E)^(-1) E'F +// +// can be re-written as +// +// sum_k F_k F_k' - F_k'E_k (E_k'E_k)^(-1) E_k' F_k +// +// Where the sum is over chunks and E_k'E_k is dense matrix of size y1 +// x y1. +// +// Advanced usage. Uptil now it has been assumed that the user would +// be interested in all of the Schur Complement S. However, it is also +// possible to use this eliminator to obtain an arbitrary submatrix of +// the full Schur complement. When the eliminator is generating the +// blocks of S, it asks the RandomAccessBlockMatrix instance passed to +// it if it has storage for that block. If it does, the eliminator +// computes/updates it, if not it is skipped. This is useful when one +// is interested in constructing a preconditioner based on the Schur +// Complement, e.g., computing the block diagonal of S so that it can +// be used as a preconditioner for an Iterative Substructuring based +// solver [See Agarwal et al, Bundle Adjustment in the Large, ECCV +// 2008 for an example of such use]. +// +// Example usage: Please see schur_complement_solver.cc +class SchurEliminatorBase { + public: + virtual ~SchurEliminatorBase() {} + + // Initialize the eliminator. It is the user's responsibilty to call + // this function before calling Eliminate or BackSubstitute. It is + // also the caller's responsibilty to ensure that the + // CompressedRowBlockStructure object passed to this method is the + // same one (or is equivalent to) the one associated with the + // BlockSparseMatrixBase objects below. + virtual void Init(int num_eliminate_blocks, + const CompressedRowBlockStructure* bs) = 0; + + // Compute the Schur complement system from the augmented linear + // least squares problem [A;D] x = [b;0]. The left hand side and the + // right hand side of the reduced linear system are returned in lhs + // and rhs respectively. + // + // It is the caller's responsibility to construct and initialize + // lhs. Depending upon the structure of the lhs object passed here, + // the full or a submatrix of the Schur complement will be computed. + // + // Since the Schur complement is a symmetric matrix, only the upper + // triangular part of the Schur complement is computed. + virtual void Eliminate(const BlockSparseMatrixBase* A, + const double* b, + const double* D, + BlockRandomAccessMatrix* lhs, + double* rhs) = 0; + + // Given values for the variables z in the F block of A, solve for + // the optimal values of the variables y corresponding to the E + // block in A. + virtual void BackSubstitute(const BlockSparseMatrixBase* A, + const double* b, + const double* D, + const double* z, + double* y) = 0; + // Factory + static SchurEliminatorBase* Create(const LinearSolver::Options& options); +}; + +// Templated implementation of the SchurEliminatorBase interface. The +// templating is on the sizes of the row, e and f blocks sizes in the +// input matrix. In many problems, the sizes of one or more of these +// blocks are constant, in that case, its worth passing these +// parameters as template arguments so that they are visible to the +// compiler and can be used for compile time optimization of the low +// level linear algebra routines. +// +// This implementation is mulithreaded using OpenMP. The level of +// parallelism is controlled by LinearSolver::Options::num_threads. +template <int kRowBlockSize = Dynamic, + int kEBlockSize = Dynamic, + int kFBlockSize = Dynamic > +class SchurEliminator : public SchurEliminatorBase { + public: + explicit SchurEliminator(const LinearSolver::Options& options) + : num_threads_(options.num_threads) { + } + + // SchurEliminatorBase Interface + virtual ~SchurEliminator(); + virtual void Init(int num_eliminate_blocks, + const CompressedRowBlockStructure* bs); + virtual void Eliminate(const BlockSparseMatrixBase* A, + const double* b, + const double* D, + BlockRandomAccessMatrix* lhs, + double* rhs); + virtual void BackSubstitute(const BlockSparseMatrixBase* A, + const double* b, + const double* D, + const double* z, + double* y); + + private: + // Chunk objects store combinatorial information needed to + // efficiently eliminate a whole chunk out of the least squares + // problem. Consider the first chunk in the example matrix above. + // + // [ y1 0 0 0 | z1 0 0 0 z5] + // [ y1 0 0 0 | z1 z2 0 0 0] + // + // One of the intermediate quantities that needs to be calculated is + // for each row the product of the y block transposed with the + // non-zero z block, and the sum of these blocks across rows. A + // temporary array "buffer_" is used for computing and storing them + // and the buffer_layout maps the indices of the z-blocks to + // position in the buffer_ array. The size of the chunk is the + // number of row blocks/residual blocks for the particular y block + // being considered. + // + // For the example chunk shown above, + // + // size = 2 + // + // The entries of buffer_layout will be filled in the following order. + // + // buffer_layout[z1] = 0 + // buffer_layout[z5] = y1 * z1 + // buffer_layout[z2] = y1 * z1 + y1 * z5 + typedef map<int, int> BufferLayoutType; + struct Chunk { + Chunk() : size(0) {} + int size; + int start; + BufferLayoutType buffer_layout; + }; + + void ChunkDiagonalBlockAndGradient( + const Chunk& chunk, + const BlockSparseMatrixBase* A, + const double* b, + int row_block_counter, + typename EigenTypes<kEBlockSize, kEBlockSize>::Matrix* eet, + typename EigenTypes<kEBlockSize>::Vector* g, + double* buffer, + BlockRandomAccessMatrix* lhs); + + void UpdateRhs(const Chunk& chunk, + const BlockSparseMatrixBase* A, + const double* b, + int row_block_counter, + const Vector& inverse_ete_g, + double* rhs); + + void ChunkOuterProduct(const CompressedRowBlockStructure* bs, + const Matrix& inverse_eet, + const double* buffer, + const BufferLayoutType& buffer_layout, + BlockRandomAccessMatrix* lhs); + void EBlockRowOuterProduct(const BlockSparseMatrixBase* A, + int row_block_index, + BlockRandomAccessMatrix* lhs); + + + void NoEBlockRowsUpdate(const BlockSparseMatrixBase* A, + const double* b, + int row_block_counter, + BlockRandomAccessMatrix* lhs, + double* rhs); + + void NoEBlockRowOuterProduct(const BlockSparseMatrixBase* A, + int row_block_index, + BlockRandomAccessMatrix* lhs); + + int num_eliminate_blocks_; + + // Block layout of the columns of the reduced linear system. Since + // the f blocks can be of varying size, this vector stores the + // position of each f block in the row/col of the reduced linear + // system. Thus lhs_row_layout_[i] is the row/col position of the + // i^th f block. + vector<int> lhs_row_layout_; + + // Combinatorial structure of the chunks in A. For more information + // see the documentation of the Chunk object above. + vector<Chunk> chunks_; + + // Buffer to store the products of the y and z blocks generated + // during the elimination phase. + scoped_array<double> buffer_; + int buffer_size_; + int num_threads_; + int uneliminated_row_begins_; + + // Locks for the blocks in the right hand side of the reduced linear + // system. + vector<Mutex*> rhs_locks_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_SCHUR_ELIMINATOR_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/schur_eliminator_impl.h b/extern/libmv/third_party/ceres/internal/ceres/schur_eliminator_impl.h new file mode 100644 index 00000000000..a388d005424 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/schur_eliminator_impl.h @@ -0,0 +1,702 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// TODO(sameeragarwal): row_block_counter can perhaps be replaced by +// Chunk::start ? + +#ifndef CERES_INTERNAL_SCHUR_ELIMINATOR_IMPL_H_ +#define CERES_INTERNAL_SCHUR_ELIMINATOR_IMPL_H_ + +#ifdef CERES_USE_OPENMP +#include <omp.h> +#endif + +// Eigen has an internal threshold switching between different matrix +// multiplication algorithms. In particular for matrices larger than +// EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD it uses a cache friendly +// matrix matrix product algorithm that has a higher setup cost. For +// matrix sizes close to this threshold, especially when the matrices +// are thin and long, the default choice may not be optimal. This is +// the case for us, as the default choice causes a 30% performance +// regression when we moved from Eigen2 to Eigen3. +#define EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD 10 + +#include <algorithm> +#include <map> +#include <glog/logging.h> +#include "Eigen/Dense" +#include "ceres/block_random_access_matrix.h" +#include "ceres/block_sparse_matrix.h" +#include "ceres/block_structure.h" +#include "ceres/map_util.h" +#include "ceres/schur_eliminator.h" +#include "ceres/stl_util.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" + +namespace ceres { +namespace internal { + +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>::~SchurEliminator() { + STLDeleteElements(&rhs_locks_); +} + +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +void +SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: +Init(int num_eliminate_blocks, const CompressedRowBlockStructure* bs) { + CHECK_GT(num_eliminate_blocks, 0) + << "SchurComplementSolver cannot be initialized with " + << "num_eliminate_blocks = 0."; + + num_eliminate_blocks_ = num_eliminate_blocks; + + const int num_col_blocks = bs->cols.size(); + const int num_row_blocks = bs->rows.size(); + + buffer_size_ = 1; + chunks_.clear(); + lhs_row_layout_.clear(); + + int lhs_num_rows = 0; + // Add a map object for each block in the reduced linear system + // and build the row/column block structure of the reduced linear + // system. + lhs_row_layout_.resize(num_col_blocks - num_eliminate_blocks_); + for (int i = num_eliminate_blocks_; i < num_col_blocks; ++i) { + lhs_row_layout_[i - num_eliminate_blocks_] = lhs_num_rows; + lhs_num_rows += bs->cols[i].size; + } + + int r = 0; + // Iterate over the row blocks of A, and detect the chunks. The + // matrix should already have been ordered so that all rows + // containing the same y block are vertically contiguous. Along + // the way also compute the amount of space each chunk will need + // to perform the elimination. + while (r < num_row_blocks) { + const int chunk_block_id = bs->rows[r].cells.front().block_id; + if (chunk_block_id >= num_eliminate_blocks_) { + break; + } + + chunks_.push_back(Chunk()); + Chunk& chunk = chunks_.back(); + chunk.size = 0; + chunk.start = r; + int buffer_size = 0; + const int e_block_size = bs->cols[chunk_block_id].size; + + // Add to the chunk until the first block in the row is + // different than the one in the first row for the chunk. + while (r + chunk.size < num_row_blocks) { + const CompressedRow& row = bs->rows[r + chunk.size]; + if (row.cells.front().block_id != chunk_block_id) { + break; + } + + // Iterate over the blocks in the row, ignoring the first + // block since it is the one to be eliminated. + for (int c = 1; c < row.cells.size(); ++c) { + const Cell& cell = row.cells[c]; + if (InsertIfNotPresent( + &(chunk.buffer_layout), cell.block_id, buffer_size)) { + buffer_size += e_block_size * bs->cols[cell.block_id].size; + } + } + + buffer_size_ = max(buffer_size, buffer_size_); + ++chunk.size; + } + + CHECK_GT(chunk.size, 0); + r += chunk.size; + } + const Chunk& chunk = chunks_.back(); + + uneliminated_row_begins_ = chunk.start + chunk.size; + if (num_threads_ > 1) { + random_shuffle(chunks_.begin(), chunks_.end()); + } + + buffer_.reset(new double[buffer_size_ * num_threads_]); + + STLDeleteElements(&rhs_locks_); + rhs_locks_.resize(num_col_blocks - num_eliminate_blocks_); + for (int i = 0; i < num_col_blocks - num_eliminate_blocks_; ++i) { + rhs_locks_[i] = new Mutex; + } + + VLOG(1) << "Eliminator threads: " << num_threads_; +} + +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +void +SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: +Eliminate(const BlockSparseMatrixBase* A, + const double* b, + const double* D, + BlockRandomAccessMatrix* lhs, + double* rhs) { + if (lhs->num_rows() > 0) { + lhs->SetZero(); + VectorRef(rhs, lhs->num_rows()).setZero(); + } + + const CompressedRowBlockStructure* bs = A->block_structure(); + const int num_col_blocks = bs->cols.size(); + + // Add the diagonal to the schur complement. + if (D != NULL) { +#pragma omp parallel for num_threads(num_threads_) schedule(dynamic) + for (int i = num_eliminate_blocks_; i < num_col_blocks; ++i) { + const int block_id = i - num_eliminate_blocks_; + int r, c, row_stride, col_stride; + CellInfo* cell_info = lhs->GetCell(block_id, block_id, + &r, &c, + &row_stride, &col_stride); + if (cell_info != NULL) { + const int block_size = bs->cols[i].size; + typename EigenTypes<kFBlockSize>::ConstVectorRef + diag(D + bs->cols[i].position, block_size); + + MutexLock l(&cell_info->m); + MatrixRef m(cell_info->values, row_stride, col_stride); + m.block(r, c, block_size, block_size).diagonal() + += diag.array().square().matrix(); + } + } + } + + // Eliminate y blocks one chunk at a time. For each chunk,x3 + // compute the entries of the normal equations and the gradient + // vector block corresponding to the y block and then apply + // Gaussian elimination to them. The matrix ete stores the normal + // matrix corresponding to the block being eliminated and array + // buffer_ contains the non-zero blocks in the row corresponding + // to this y block in the normal equations. This computation is + // done in ChunkDiagonalBlockAndGradient. UpdateRhs then applies + // gaussian elimination to the rhs of the normal equations, + // updating the rhs of the reduced linear system by modifying rhs + // blocks for all the z blocks that share a row block/residual + // term with the y block. EliminateRowOuterProduct does the + // corresponding operation for the lhs of the reduced linear + // system. +#pragma omp parallel for num_threads(num_threads_) schedule(dynamic) + for (int i = 0; i < chunks_.size(); ++i) { +#ifdef CERES_USE_OPENMP + int thread_id = omp_get_thread_num(); +#else + int thread_id = 0; +#endif + double* buffer = buffer_.get() + thread_id * buffer_size_; + const Chunk& chunk = chunks_[i]; + const int e_block_id = bs->rows[chunk.start].cells.front().block_id; + const int e_block_size = bs->cols[e_block_id].size; + + VectorRef(buffer, buffer_size_).setZero(); + + typename EigenTypes<kEBlockSize, kEBlockSize>::Matrix + ete(e_block_size, e_block_size); + + if (D != NULL) { + const typename EigenTypes<kEBlockSize>::ConstVectorRef + diag(D + bs->cols[e_block_id].position, e_block_size); + ete = diag.array().square().matrix().asDiagonal(); + } else { + ete.setZero(); + } + + typename EigenTypes<kEBlockSize>::Vector g(e_block_size); + g.setZero(); + + // We are going to be computing + // + // S += F'F - F'E(E'E)^{-1}E'F + // + // for each Chunk. The computation is broken down into a number of + // function calls as below. + + // Compute the outer product of the e_blocks with themselves (ete + // = E'E). Compute the product of the e_blocks with the + // corresonding f_blocks (buffer = E'F), the gradient of the terms + // in this chunk (g) and add the outer product of the f_blocks to + // Schur complement (S += F'F). + ChunkDiagonalBlockAndGradient( + chunk, A, b, chunk.start, &ete, &g, buffer, lhs); + + // Normally one wouldn't compute the inverse explicitly, but + // e_block_size will typically be a small number like 3, in + // which case its much faster to compute the inverse once and + // use it to multiply other matrices/vectors instead of doing a + // Solve call over and over again. + typename EigenTypes<kEBlockSize, kEBlockSize>::Matrix inverse_ete = + ete + .template selfadjointView<Eigen::Upper>() + .ldlt() + .solve(Matrix::Identity(e_block_size, e_block_size)); + + // For the current chunk compute and update the rhs of the reduced + // linear system. + // + // rhs = F'b - F'E(E'E)^(-1) E'b + UpdateRhs(chunk, A, b, chunk.start, inverse_ete * g, rhs); + + // S -= F'E(E'E)^{-1}E'F + ChunkOuterProduct(bs, inverse_ete, buffer, chunk.buffer_layout, lhs); + } + + // For rows with no e_blocks, the schur complement update reduces to + // S += F'F. + NoEBlockRowsUpdate(A, b, uneliminated_row_begins_, lhs, rhs); +} + +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +void +SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: +BackSubstitute(const BlockSparseMatrixBase* A, + const double* b, + const double* D, + const double* z, + double* y) { + const CompressedRowBlockStructure* bs = A->block_structure(); +#pragma omp parallel for num_threads(num_threads_) schedule(dynamic) + for (int i = 0; i < chunks_.size(); ++i) { + const Chunk& chunk = chunks_[i]; + const int e_block_id = bs->rows[chunk.start].cells.front().block_id; + const int e_block_size = bs->cols[e_block_id].size; + + typename EigenTypes<kEBlockSize>::VectorRef y_block( + y + bs->cols[e_block_id].position, e_block_size); + + typename EigenTypes<kEBlockSize, kEBlockSize>::Matrix + ete(e_block_size, e_block_size); + if (D != NULL) { + const typename EigenTypes<kEBlockSize>::ConstVectorRef + diag(D + bs->cols[e_block_id].position, e_block_size); + ete = diag.array().square().matrix().asDiagonal(); + } else { + ete.setZero(); + } + + for (int j = 0; j < chunk.size; ++j) { + const CompressedRow& row = bs->rows[chunk.start + j]; + const double* row_values = A->RowBlockValues(chunk.start + j); + const Cell& e_cell = row.cells.front(); + DCHECK_EQ(e_block_id, e_cell.block_id); + const typename EigenTypes<kRowBlockSize, kEBlockSize>::ConstMatrixRef + e_block(row_values + e_cell.position, + row.block.size, + e_block_size); + + typename EigenTypes<kRowBlockSize>::Vector + sj = + typename EigenTypes<kRowBlockSize>::ConstVectorRef + (b + bs->rows[chunk.start + j].block.position, + row.block.size); + + for (int c = 1; c < row.cells.size(); ++c) { + const int f_block_id = row.cells[c].block_id; + const int f_block_size = bs->cols[f_block_id].size; + const typename EigenTypes<kRowBlockSize, kFBlockSize>::ConstMatrixRef + f_block(row_values + row.cells[c].position, + row.block.size, f_block_size); + const int r_block = f_block_id - num_eliminate_blocks_; + + sj -= f_block * + typename EigenTypes<kFBlockSize>::ConstVectorRef + (z + lhs_row_layout_[r_block], f_block_size); + } + + y_block += e_block.transpose() * sj; + ete.template selfadjointView<Eigen::Upper>() + .rankUpdate(e_block.transpose(), 1.0); + } + + y_block = + ete + .template selfadjointView<Eigen::Upper>() + .ldlt() + .solve(y_block); + } +} + +// Update the rhs of the reduced linear system. Compute +// +// F'b - F'E(E'E)^(-1) E'b +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +void +SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: +UpdateRhs(const Chunk& chunk, + const BlockSparseMatrixBase* A, + const double* b, + int row_block_counter, + const Vector& inverse_ete_g, + double* rhs) { + const CompressedRowBlockStructure* bs = A->block_structure(); + const int e_block_size = inverse_ete_g.rows(); + int b_pos = bs->rows[row_block_counter].block.position; + for (int j = 0; j < chunk.size; ++j) { + const CompressedRow& row = bs->rows[row_block_counter + j]; + const double *row_values = A->RowBlockValues(row_block_counter + j); + const Cell& e_cell = row.cells.front(); + + const typename EigenTypes<kRowBlockSize, kEBlockSize>::ConstMatrixRef + e_block(row_values + e_cell.position, + row.block.size, + e_block_size); + + const typename EigenTypes<kRowBlockSize>::Vector + sj = + typename EigenTypes<kRowBlockSize>::ConstVectorRef + (b + b_pos, row.block.size) - e_block * (inverse_ete_g); + + for (int c = 1; c < row.cells.size(); ++c) { + const int block_id = row.cells[c].block_id; + const int block_size = bs->cols[block_id].size; + const typename EigenTypes<kRowBlockSize, kFBlockSize>::ConstMatrixRef + b(row_values + row.cells[c].position, + row.block.size, block_size); + + const int block = block_id - num_eliminate_blocks_; + MutexLock l(rhs_locks_[block]); + typename EigenTypes<kFBlockSize>::VectorRef + (rhs + lhs_row_layout_[block], block_size).noalias() + += b.transpose() * sj; + } + b_pos += row.block.size; + } +} + +// Given a Chunk - set of rows with the same e_block, e.g. in the +// following Chunk with two rows. +// +// E F +// [ y11 0 0 0 | z11 0 0 0 z51] +// [ y12 0 0 0 | z12 z22 0 0 0] +// +// this function computes twp matrices. The diagonal block matrix +// +// ete = y11 * y11' + y12 * y12' +// +// and the off diagonal blocks in the Guass Newton Hessian. +// +// buffer = [y11'(z11 + z12), y12' * z22, y11' * z51] +// +// which are zero compressed versions of the block sparse matrices E'E +// and E'F. +// +// and the gradient of the e_block, E'b. +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +void +SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: +ChunkDiagonalBlockAndGradient( + const Chunk& chunk, + const BlockSparseMatrixBase* A, + const double* b, + int row_block_counter, + typename EigenTypes<kEBlockSize, kEBlockSize>::Matrix* ete, + typename EigenTypes<kEBlockSize>::Vector* g, + double* buffer, + BlockRandomAccessMatrix* lhs) { + const CompressedRowBlockStructure* bs = A->block_structure(); + + int b_pos = bs->rows[row_block_counter].block.position; + const int e_block_size = ete->rows(); + + // Iterate over the rows in this chunk, for each row, compute the + // contribution of its F blocks to the Schur complement, the + // contribution of its E block to the matrix EE' (ete), and the + // corresponding block in the gradient vector. + for (int j = 0; j < chunk.size; ++j) { + const CompressedRow& row = bs->rows[row_block_counter + j]; + const double *row_values = A->RowBlockValues(row_block_counter + j); + + if (row.cells.size() > 1) { + EBlockRowOuterProduct(A, row_block_counter + j, lhs); + } + + // Extract the e_block, ETE += E_i' E_i + const Cell& e_cell = row.cells.front(); + const typename EigenTypes<kRowBlockSize, kEBlockSize>::ConstMatrixRef + e_block(row_values + e_cell.position, + row.block.size, + e_block_size); + + ete->template selfadjointView<Eigen::Upper>() + .rankUpdate(e_block.transpose(), 1.0); + + // g += E_i' b_i + g->noalias() += e_block.transpose() * + typename EigenTypes<kRowBlockSize>::ConstVectorRef + (b + b_pos, row.block.size); + + // buffer = E'F. This computation is done by iterating over the + // f_blocks for each row in the chunk. + for (int c = 1; c < row.cells.size(); ++c) { + const int f_block_id = row.cells[c].block_id; + const int f_block_size = bs->cols[f_block_id].size; + const typename EigenTypes<kRowBlockSize, kFBlockSize>::ConstMatrixRef + f_block(row_values + row.cells[c].position, + row.block.size, f_block_size); + + double* buffer_ptr = + buffer + FindOrDie(chunk.buffer_layout, f_block_id); + + typename EigenTypes<kEBlockSize, kFBlockSize>::MatrixRef + (buffer_ptr, e_block_size, f_block_size).noalias() + += e_block.transpose() * f_block; + } + b_pos += row.block.size; + } +} + +// Compute the outer product F'E(E'E)^{-1}E'F and subtract it from the +// Schur complement matrix, i.e +// +// S -= F'E(E'E)^{-1}E'F. +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +void +SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: +ChunkOuterProduct(const CompressedRowBlockStructure* bs, + const Matrix& inverse_ete, + const double* buffer, + const BufferLayoutType& buffer_layout, + BlockRandomAccessMatrix* lhs) { + // This is the most computationally expensive part of this + // code. Profiling experiments reveal that the bottleneck is not the + // computation of the right-hand matrix product, but memory + // references to the left hand side. + const int e_block_size = inverse_ete.rows(); + BufferLayoutType::const_iterator it1 = buffer_layout.begin(); + // S(i,j) -= bi' * ete^{-1} b_j + for (; it1 != buffer_layout.end(); ++it1) { + const int block1 = it1->first - num_eliminate_blocks_; + const int block1_size = bs->cols[it1->first].size; + + const typename EigenTypes<kEBlockSize, kFBlockSize>::ConstMatrixRef + b1(buffer + it1->second, e_block_size, block1_size); + const typename EigenTypes<kFBlockSize, kEBlockSize>::Matrix + b1_transpose_inverse_ete = b1.transpose() * inverse_ete; + + BufferLayoutType::const_iterator it2 = it1; + for (; it2 != buffer_layout.end(); ++it2) { + const int block2 = it2->first - num_eliminate_blocks_; + + int r, c, row_stride, col_stride; + CellInfo* cell_info = lhs->GetCell(block1, block2, + &r, &c, + &row_stride, &col_stride); + if (cell_info == NULL) { + continue; + } + + const int block2_size = bs->cols[it2->first].size; + const typename EigenTypes<kEBlockSize, kFBlockSize>::ConstMatrixRef + b2(buffer + it2->second, e_block_size, block2_size); + + MutexLock l(&cell_info->m); + MatrixRef m(cell_info->values, row_stride, col_stride); + + // We explicitly construct a block object here instead of using + // m.block(), as m.block() variant of the constructor does not + // allow mixing of template sizing and runtime sizing parameters + // like the Matrix class does. + Eigen::Block<MatrixRef, kFBlockSize, kFBlockSize> + block(m, r, c, block1_size, block2_size); + block.noalias() -= b1_transpose_inverse_ete * b2; + } + } +} + +// For rows with no e_blocks, the schur complement update reduces to S +// += F'F. This function iterates over the rows of A with no e_block, +// and calls NoEBlockRowOuterProduct on each row. +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +void +SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: +NoEBlockRowsUpdate(const BlockSparseMatrixBase* A, + const double* b, + int row_block_counter, + BlockRandomAccessMatrix* lhs, + double* rhs) { + const CompressedRowBlockStructure* bs = A->block_structure(); + for (; row_block_counter < bs->rows.size(); ++row_block_counter) { + const CompressedRow& row = bs->rows[row_block_counter]; + const double *row_values = A->RowBlockValues(row_block_counter); + for (int c = 0; c < row.cells.size(); ++c) { + const int block_id = row.cells[c].block_id; + const int block_size = bs->cols[block_id].size; + const int block = block_id - num_eliminate_blocks_; + VectorRef(rhs + lhs_row_layout_[block], block_size).noalias() + += (ConstMatrixRef(row_values + row.cells[c].position, + row.block.size, block_size).transpose() * + ConstVectorRef(b + row.block.position, row.block.size)); + } + NoEBlockRowOuterProduct(A, row_block_counter, lhs); + } +} + + +// A row r of A, which has no e_blocks gets added to the Schur +// Complement as S += r r'. This function is responsible for computing +// the contribution of a single row r to the Schur complement. It is +// very similar in structure to EBlockRowOuterProduct except for +// one difference. It does not use any of the template +// parameters. This is because the algorithm used for detecting the +// static structure of the matrix A only pays attention to rows with +// e_blocks. This is becase rows without e_blocks are rare and +// typically arise from regularization terms in the original +// optimization problem, and have a very different structure than the +// rows with e_blocks. Including them in the static structure +// detection will lead to most template parameters being set to +// dynamic. Since the number of rows without e_blocks is small, the +// lack of templating is not an issue. +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +void +SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: +NoEBlockRowOuterProduct(const BlockSparseMatrixBase* A, + int row_block_index, + BlockRandomAccessMatrix* lhs) { + const CompressedRowBlockStructure* bs = A->block_structure(); + const CompressedRow& row = bs->rows[row_block_index]; + const double *row_values = A->RowBlockValues(row_block_index); + for (int i = 0; i < row.cells.size(); ++i) { + const int block1 = row.cells[i].block_id - num_eliminate_blocks_; + DCHECK_GE(block1, 0); + + const int block1_size = bs->cols[row.cells[i].block_id].size; + const ConstMatrixRef b1(row_values + row.cells[i].position, + row.block.size, block1_size); + int r, c, row_stride, col_stride; + CellInfo* cell_info = lhs->GetCell(block1, block1, + &r, &c, + &row_stride, &col_stride); + if (cell_info != NULL) { + MutexLock l(&cell_info->m); + MatrixRef m(cell_info->values, row_stride, col_stride); + m.block(r, c, block1_size, block1_size) + .selfadjointView<Eigen::Upper>() + .rankUpdate(b1.transpose(), 1.0); + } + + for (int j = i + 1; j < row.cells.size(); ++j) { + const int block2 = row.cells[j].block_id - num_eliminate_blocks_; + DCHECK_GE(block2, 0); + DCHECK_LT(block1, block2); + int r, c, row_stride, col_stride; + CellInfo* cell_info = lhs->GetCell(block1, block2, + &r, &c, + &row_stride, &col_stride); + if (cell_info == NULL) { + continue; + } + + const int block2_size = bs->cols[row.cells[j].block_id].size; + MutexLock l(&cell_info->m); + MatrixRef m(cell_info->values, row_stride, col_stride); + m.block(r, c, block1_size, block2_size).noalias() += + b1.transpose() * ConstMatrixRef(row_values + row.cells[j].position, + row.block.size, + block2_size); + } + } +} + +// For a row with an e_block, compute the contribition S += F'F. This +// function has the same structure as NoEBlockRowOuterProduct, except +// that this function uses the template parameters. +template <int kRowBlockSize, int kEBlockSize, int kFBlockSize> +void +SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>:: +EBlockRowOuterProduct(const BlockSparseMatrixBase* A, + int row_block_index, + BlockRandomAccessMatrix* lhs) { + const CompressedRowBlockStructure* bs = A->block_structure(); + const CompressedRow& row = bs->rows[row_block_index]; + const double *row_values = A->RowBlockValues(row_block_index); + for (int i = 1; i < row.cells.size(); ++i) { + const int block1 = row.cells[i].block_id - num_eliminate_blocks_; + DCHECK_GE(block1, 0); + + const int block1_size = bs->cols[row.cells[i].block_id].size; + const typename EigenTypes<kRowBlockSize, kFBlockSize>::ConstMatrixRef + b1(row_values + row.cells[i].position, + row.block.size, block1_size); + { + int r, c, row_stride, col_stride; + CellInfo* cell_info = lhs->GetCell(block1, block1, + &r, &c, + &row_stride, &col_stride); + if (cell_info == NULL) { + continue; + } + + MutexLock l(&cell_info->m); + MatrixRef m(cell_info->values, row_stride, col_stride); + + Eigen::Block<MatrixRef, kFBlockSize, kFBlockSize> + block(m, r, c, block1_size, block1_size); + block.template selfadjointView<Eigen::Upper>() + .rankUpdate(b1.transpose(), 1.0); + } + + for (int j = i + 1; j < row.cells.size(); ++j) { + const int block2 = row.cells[j].block_id - num_eliminate_blocks_; + DCHECK_GE(block2, 0); + DCHECK_LT(block1, block2); + const int block2_size = bs->cols[row.cells[j].block_id].size; + int r, c, row_stride, col_stride; + CellInfo* cell_info = lhs->GetCell(block1, block2, + &r, &c, + &row_stride, &col_stride); + if (cell_info == NULL) { + continue; + } + + const typename EigenTypes<kRowBlockSize, kFBlockSize>::ConstMatrixRef + b2(row_values + row.cells[j].position, + row.block.size, + block2_size); + + MutexLock l(&cell_info->m); + MatrixRef m(cell_info->values, row_stride, col_stride); + Eigen::Block<MatrixRef, kFBlockSize, kFBlockSize> + block(m, r, c, block1_size, block2_size); + block.noalias() += b1.transpose() * b2; + } + } +} + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_SCHUR_ELIMINATOR_IMPL_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/schur_ordering.cc b/extern/libmv/third_party/ceres/internal/ceres/schur_ordering.cc new file mode 100644 index 00000000000..c4fc1da3c2f --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/schur_ordering.cc @@ -0,0 +1,113 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/schur_ordering.h" + +#include <glog/logging.h> +#include "ceres/graph.h" +#include "ceres/graph_algorithms.h" +#include "ceres/map_util.h" +#include "ceres/parameter_block.h" +#include "ceres/program.h" +#include "ceres/residual_block.h" +#include "ceres/internal/scoped_ptr.h" + +CERES_HASH_NAMESPACE_START + +// Allow us to hash pointers as if they were int's +template<> struct hash< ::ceres::internal::ParameterBlock*> { + size_t operator()(::ceres::internal::ParameterBlock* x) const { + return reinterpret_cast<size_t>(x); + } +}; + +CERES_HASH_NAMESPACE_END + +namespace ceres { +namespace internal { + +int ComputeSchurOrdering(const Program& program, + vector<ParameterBlock*>* ordering) { + CHECK_NOTNULL(ordering)->clear(); + + scoped_ptr<Graph< ParameterBlock*> > graph( + CHECK_NOTNULL(CreateHessianGraph(program))); + int independent_set_size = + IndependentSetOrdering<ParameterBlock*>(*graph, ordering); + const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks(); + + // Add the excluded blocks to back of the ordering vector. + for (int i = 0; i < parameter_blocks.size(); ++i) { + ParameterBlock* parameter_block = parameter_blocks[i]; + if (parameter_block->IsConstant()) { + ordering->push_back(parameter_block); + } + } + + return independent_set_size; +} + +Graph<ParameterBlock*>* +CreateHessianGraph(const Program& program) { + Graph<ParameterBlock*>* graph = new Graph<ParameterBlock*>; + const vector<ParameterBlock*>& parameter_blocks = program.parameter_blocks(); + for (int i = 0; i < parameter_blocks.size(); ++i) { + ParameterBlock* parameter_block = parameter_blocks[i]; + if (!parameter_block->IsConstant()) { + graph->AddVertex(parameter_block); + } + } + + const vector<ResidualBlock*>& residual_blocks = program.residual_blocks(); + for (int i = 0; i < residual_blocks.size(); ++i) { + const ResidualBlock* residual_block = residual_blocks[i]; + const int num_parameter_blocks = residual_block->NumParameterBlocks(); + ParameterBlock* const* parameter_blocks = + residual_block->parameter_blocks(); + for (int j = 0; j < num_parameter_blocks; ++j) { + if (parameter_blocks[j]->IsConstant()) { + continue; + } + + for (int k = j + 1; k < num_parameter_blocks; ++k) { + if (parameter_blocks[k]->IsConstant()) { + continue; + } + + graph->AddEdge(parameter_blocks[j], parameter_blocks[k]); + } + } + } + + return graph; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/schur_ordering.h b/extern/libmv/third_party/ceres/internal/ceres/schur_ordering.h new file mode 100644 index 00000000000..1f9a4ff354f --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/schur_ordering.h @@ -0,0 +1,74 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Compute a parameter block ordering for use with the Schur +// complement based algorithms. + +#ifndef CERES_INTERNAL_SCHUR_ORDERING_H_ +#define CERES_INTERNAL_SCHUR_ORDERING_H_ + +#include <vector> +#include "ceres/graph.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +class Program; +class ParameterBlock; + +// Uses an approximate independent set ordering to order the parameter +// blocks of a problem so that it is suitable for use with Schur +// complement based solvers. The output variable ordering contains an +// ordering of the parameter blocks and the return value is size of +// the independent set or the number of e_blocks (see +// schur_complement_solver.h for an explanation). Constant parameters +// are added to the end. +// +// The ordering vector has the structure +// +// ordering = [independent set, +// complement of the independent set, +// fixed blocks] +int ComputeSchurOrdering(const Program& program, + vector<ParameterBlock* >* ordering); + + +// Builds a graph on the parameter blocks of a Problem, whose +// structure reflects the sparsity structure of the Hessian. Each +// vertex corresponds to a parameter block in the Problem except for +// parameter blocks that are marked constant. An edge connects two +// parameter blocks, if they co-occur in a residual block. +Graph<ParameterBlock*>* CreateHessianGraph(const Program& program); + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_SCHUR_ORDERING_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/scratch_evaluate_preparer.cc b/extern/libmv/third_party/ceres/internal/ceres/scratch_evaluate_preparer.cc new file mode 100644 index 00000000000..6f0ceefd87d --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/scratch_evaluate_preparer.cc @@ -0,0 +1,78 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#include "ceres/scratch_evaluate_preparer.h" + +#include "ceres/parameter_block.h" +#include "ceres/program.h" +#include "ceres/residual_block.h" + +namespace ceres { +namespace internal { + +ScratchEvaluatePreparer* ScratchEvaluatePreparer::Create( + const Program &program, + int num_threads) { + ScratchEvaluatePreparer* preparers = new ScratchEvaluatePreparer[num_threads]; + int max_derivatives_per_residual_block = + program.MaxDerivativesPerResidualBlock(); + for (int i = 0; i < num_threads; i++) { + preparers[i].Init(max_derivatives_per_residual_block); + } + return preparers; +} + +void ScratchEvaluatePreparer::Init(int max_derivatives_per_residual_block) { + jacobian_scratch_.reset( + new double[max_derivatives_per_residual_block]); +} + +// Point the jacobian blocks into the scratch area of this evaluate preparer. +void ScratchEvaluatePreparer::Prepare(const ResidualBlock* residual_block, + int /* residual_block_index */, + SparseMatrix* /* jacobian */, + double** jacobians) { + double* jacobian_block_cursor = jacobian_scratch_.get(); + int num_residuals = residual_block->NumResiduals(); + int num_parameter_blocks = residual_block->NumParameterBlocks(); + for (int j = 0; j < num_parameter_blocks; ++j) { + const ParameterBlock* parameter_block = + residual_block->parameter_blocks()[j]; + if (parameter_block->IsConstant()) { + jacobians[j] = NULL; + } else { + jacobians[j] = jacobian_block_cursor; + jacobian_block_cursor += num_residuals * parameter_block->LocalSize(); + } + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/scratch_evaluate_preparer.h b/extern/libmv/third_party/ceres/internal/ceres/scratch_evaluate_preparer.h new file mode 100644 index 00000000000..6b127081976 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/scratch_evaluate_preparer.h @@ -0,0 +1,69 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// +// A scratch evaluate preparer provides temporary storage for the jacobians that +// are created when running user-provided cost functions. The evaluator takes +// care to avoid evaluating the jacobian for fixed parameters. + +#ifndef CERES_INTERNAL_SCRATCH_EVALUATE_PREPARER_H_ +#define CERES_INTERNAL_SCRATCH_EVALUATE_PREPARER_H_ + +#include "ceres/internal/scoped_ptr.h" + +namespace ceres { +namespace internal { + +class Program; +class ResidualBlock; +class SparseMatrix; + +class ScratchEvaluatePreparer { + public: + // Create num_threads ScratchEvaluatePreparers. + static ScratchEvaluatePreparer* Create(const Program &program, + int num_threads); + + // EvaluatePreparer interface + void Init(int max_derivatives_per_residual_block); + void Prepare(const ResidualBlock* residual_block, + int residual_block_index, + SparseMatrix* jacobian, + double** jacobians); + + private: + // Scratch space for the jacobians; each jacobian is packed one after another. + // There is enough scratch to hold all the jacobians for the largest residual. + scoped_array<double> jacobian_scratch_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_SCRATCH_EVALUATE_PREPARER_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/solver.cc b/extern/libmv/third_party/ceres/internal/ceres/solver.cc new file mode 100644 index 00000000000..77f04d1d918 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/solver.cc @@ -0,0 +1,230 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) +// sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/solver.h" + +#include <vector> +#include "ceres/levenberg_marquardt.h" +#include "ceres/program.h" +#include "ceres/solver_impl.h" +#include "ceres/stringprintf.h" +#include "ceres/problem.h" + +namespace ceres { + +Solver::~Solver() {} + +// TODO(sameeragarwal): The timing code here should use a sub-second +// timer. +void Solver::Solve(const Solver::Options& options, + Problem* problem, + Solver::Summary* summary) { + time_t start_time_seconds = time(NULL); + internal::SolverImpl::Solve(options, problem, summary); + summary->total_time_in_seconds = time(NULL) - start_time_seconds; + summary->preprocessor_time_in_seconds = + summary->total_time_in_seconds - summary->minimizer_time_in_seconds; +} + +void Solve(const Solver::Options& options, + Problem* problem, + Solver::Summary* summary) { + time_t start_time_seconds = time(NULL); + internal::SolverImpl::Solve(options, problem, summary); + summary->total_time_in_seconds = time(NULL) - start_time_seconds; + summary->preprocessor_time_in_seconds = + summary->total_time_in_seconds - summary->minimizer_time_in_seconds; +} + +Solver::Summary::Summary() + // Invalid values for most fields, to ensure that we are not + // accidentally reporting default values. + : termination_type(DID_NOT_RUN), + initial_cost(-1.0), + final_cost(-1.0), + fixed_cost(-1.0), + num_successful_steps(-1), + num_unsuccessful_steps(-1), + preprocessor_time_in_seconds(-1.0), + minimizer_time_in_seconds(-1.0), + total_time_in_seconds(-1.0), + num_parameter_blocks(-1), + num_parameters(-1), + num_residual_blocks(-1), + num_residuals(-1), + num_parameter_blocks_reduced(-1), + num_parameters_reduced(-1), + num_residual_blocks_reduced(-1), + num_residuals_reduced(-1), + num_eliminate_blocks_given(-1), + num_eliminate_blocks_used(-1), + num_threads_given(-1), + num_threads_used(-1), + num_linear_solver_threads_given(-1), + num_linear_solver_threads_used(-1), + linear_solver_type_given(SPARSE_NORMAL_CHOLESKY), + linear_solver_type_used(SPARSE_NORMAL_CHOLESKY), + preconditioner_type(IDENTITY), + ordering_type(NATURAL) { +} + +string Solver::Summary::BriefReport() const { + string report = "Ceres Solver Report: "; + if (termination_type == DID_NOT_RUN) { + CHECK(!error.empty()) + << "Solver terminated with DID_NOT_RUN but the solver did not " + << "return a reason. This is a Ceres error. Please report this " + << "to the Ceres team"; + return report + "Termination: DID_NOT_RUN, because " + error; + } + + internal::StringAppendF(&report, "Iterations: %d", + num_successful_steps + num_unsuccessful_steps); + internal::StringAppendF(&report, ", Initial cost: %e", initial_cost); + + // If the solver failed or was aborted, then the final_cost has no + // meaning. + if (termination_type != NUMERICAL_FAILURE && + termination_type != USER_ABORT) { + internal::StringAppendF(&report, ", Final cost: %e", final_cost); + } + + internal::StringAppendF(&report, ", Termination: %s.", + SolverTerminationTypeToString(termination_type)); + return report; +}; + +string Solver::Summary::FullReport() const { + string report = + "\n" + "Ceres Solver Report\n" + "-------------------\n"; + + if (termination_type == DID_NOT_RUN) { + internal::StringAppendF(&report, " Original\n"); + internal::StringAppendF(&report, "Parameter blocks % 10d\n", + num_parameter_blocks); + internal::StringAppendF(&report, "Parameters % 10d\n", + num_parameters); + internal::StringAppendF(&report, "Residual blocks % 10d\n", + num_residual_blocks); + internal::StringAppendF(&report, "Residual % 10d\n\n", + num_residuals); + } else { + internal::StringAppendF(&report, "%45s %21s\n", "Original", "Reduced"); + internal::StringAppendF(&report, "Parameter blocks % 25d% 25d\n", + num_parameter_blocks, num_parameter_blocks_reduced); + internal::StringAppendF(&report, "Parameters % 25d% 25d\n", + num_parameters, num_parameters_reduced); + internal::StringAppendF(&report, "Residual blocks % 25d% 25d\n", + num_residual_blocks, num_residual_blocks_reduced); + internal::StringAppendF(&report, "Residual % 25d% 25d\n\n", + num_residuals, num_residuals_reduced); + } + + internal::StringAppendF(&report, "%45s %21s\n", "Given", "Used"); + internal::StringAppendF(&report, "Linear solver %25s%25s\n", + LinearSolverTypeToString(linear_solver_type_given), + LinearSolverTypeToString(linear_solver_type_used)); + + if (linear_solver_type_given == CGNR || + linear_solver_type_given == ITERATIVE_SCHUR) { + internal::StringAppendF(&report, "Preconditioner %25s%25s\n", + PreconditionerTypeToString(preconditioner_type), + PreconditionerTypeToString(preconditioner_type)); + } else { + internal::StringAppendF(&report, "Preconditioner %25s%25s\n", + "N/A", "N/A"); + } + + internal::StringAppendF(&report, "Ordering %25s%25s\n", + OrderingTypeToString(ordering_type), + OrderingTypeToString(ordering_type)); + + if (IsSchurType(linear_solver_type_given)) { + if (ordering_type == SCHUR) { + internal::StringAppendF(&report, "num_eliminate_blocks%25s% 25d\n", + "N/A", + num_eliminate_blocks_used); + } else { + internal::StringAppendF(&report, "num_eliminate_blocks% 25d% 25d\n", + num_eliminate_blocks_given, + num_eliminate_blocks_used); + } + } + + internal::StringAppendF(&report, "Threads: % 25d% 25d\n", + num_threads_given, num_threads_used); + internal::StringAppendF(&report, "Linear Solver Threads:% 23d% 25d\n", + num_linear_solver_threads_given, + num_linear_solver_threads_used); + + + if (termination_type == DID_NOT_RUN) { + CHECK(!error.empty()) + << "Solver terminated with DID_NOT_RUN but the solver did not " + << "return a reason. This is a Ceres error. Please report this " + << "to the Ceres team"; + internal::StringAppendF(&report, "Termination: %20s\n", + "DID_NOT_RUN"); + internal::StringAppendF(&report, "Reason: %s\n", error.c_str()); + return report; + } + + internal::StringAppendF(&report, "\nCost:\n"); + internal::StringAppendF(&report, "Initial % 30e\n", initial_cost); + if (termination_type != NUMERICAL_FAILURE && termination_type != USER_ABORT) { + internal::StringAppendF(&report, "Final % 30e\n", final_cost); + internal::StringAppendF(&report, "Change % 30e\n", + initial_cost - final_cost); + } + + internal::StringAppendF(&report, "\nNumber of iterations:\n"); + internal::StringAppendF(&report, "Successful % 20d\n", + num_successful_steps); + internal::StringAppendF(&report, "Unsuccessful % 20d\n", + num_unsuccessful_steps); + internal::StringAppendF(&report, "Total % 20d\n", + num_successful_steps + num_unsuccessful_steps); + internal::StringAppendF(&report, "\nTime (in seconds):\n"); + internal::StringAppendF(&report, "Preprocessor % 25e\n", + preprocessor_time_in_seconds); + internal::StringAppendF(&report, "Minimizer % 25e\n", + minimizer_time_in_seconds); + internal::StringAppendF(&report, "Total % 25e\n", + total_time_in_seconds); + + internal::StringAppendF(&report, "Termination: %25s\n", + SolverTerminationTypeToString(termination_type)); + return report; +}; + +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/solver_impl.cc b/extern/libmv/third_party/ceres/internal/ceres/solver_impl.cc new file mode 100644 index 00000000000..ed07d9dc6d7 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/solver_impl.cc @@ -0,0 +1,693 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#include "ceres/solver_impl.h" + +#include <iostream> // NOLINT +#include <numeric> +#include "ceres/evaluator.h" +#include "ceres/gradient_checking_cost_function.h" +#include "ceres/levenberg_marquardt.h" +#include "ceres/linear_solver.h" +#include "ceres/map_util.h" +#include "ceres/minimizer.h" +#include "ceres/parameter_block.h" +#include "ceres/problem_impl.h" +#include "ceres/program.h" +#include "ceres/residual_block.h" +#include "ceres/schur_ordering.h" +#include "ceres/stringprintf.h" +#include "ceres/iteration_callback.h" +#include "ceres/problem.h" + +namespace ceres { +namespace internal { +namespace { + +void EvaluateCostAndResiduals(ProblemImpl* problem_impl, + double* cost, + vector<double>* residuals) { + CHECK_NOTNULL(cost); + Program* program = CHECK_NOTNULL(problem_impl)->mutable_program(); + if (residuals != NULL) { + residuals->resize(program->NumResiduals()); + program->Evaluate(cost, &(*residuals)[0]); + } else { + program->Evaluate(cost, NULL); + } +} + +// Callback for updating the user's parameter blocks. Updates are only +// done if the step is successful. +class StateUpdatingCallback : public IterationCallback { + public: + StateUpdatingCallback(Program* program, double* parameters) + : program_(program), parameters_(parameters) {} + + CallbackReturnType operator()(const IterationSummary& summary) { + if (summary.step_is_successful) { + program_->StateVectorToParameterBlocks(parameters_); + program_->CopyParameterBlockStateToUserState(); + } + return SOLVER_CONTINUE; + } + + private: + Program* program_; + double* parameters_; +}; + +// Callback for logging the state of the minimizer to STDERR or STDOUT +// depending on the user's preferences and logging level. +class LoggingCallback : public IterationCallback { + public: + explicit LoggingCallback(bool log_to_stdout) + : log_to_stdout_(log_to_stdout) {} + + ~LoggingCallback() {} + + CallbackReturnType operator()(const IterationSummary& summary) { + const char* kReportRowFormat = + "% 4d: f:% 8e d:% 3.2e g:% 3.2e h:% 3.2e " + "rho:% 3.2e mu:% 3.2e li:% 3d"; + string output = StringPrintf(kReportRowFormat, + summary.iteration, + summary.cost, + summary.cost_change, + summary.gradient_max_norm, + summary.step_norm, + summary.relative_decrease, + summary.mu, + summary.linear_solver_iterations); + if (log_to_stdout_) { + cout << output << endl; + } else { + VLOG(1) << output; + } + return SOLVER_CONTINUE; + } + + private: + const bool log_to_stdout_; +}; + +} // namespace + +void SolverImpl::Minimize(const Solver::Options& options, + Program* program, + Evaluator* evaluator, + LinearSolver* linear_solver, + double* initial_parameters, + double* final_parameters, + Solver::Summary* summary) { + Minimizer::Options minimizer_options(options); + + LoggingCallback logging_callback(options.minimizer_progress_to_stdout); + if (options.logging_type != SILENT) { + minimizer_options.callbacks.push_back(&logging_callback); + } + + StateUpdatingCallback updating_callback(program, initial_parameters); + if (options.update_state_every_iteration) { + minimizer_options.callbacks.push_back(&updating_callback); + } + + LevenbergMarquardt levenberg_marquardt; + + time_t start_minimizer_time_seconds = time(NULL); + levenberg_marquardt.Minimize(minimizer_options, + evaluator, + linear_solver, + initial_parameters, + final_parameters, + summary); + summary->minimizer_time_in_seconds = + time(NULL) - start_minimizer_time_seconds; +} + +void SolverImpl::Solve(const Solver::Options& original_options, + Problem* problem, + Solver::Summary* summary) { + Solver::Options options(original_options); + +#ifndef CERES_USE_OPENMP + if (options.num_threads > 1) { + LOG(WARNING) + << "OpenMP support is not compiled into this binary; " + << "only options.num_threads=1 is supported. Switching" + << "to single threaded mode."; + options.num_threads = 1; + } + if (options.num_linear_solver_threads > 1) { + LOG(WARNING) + << "OpenMP support is not compiled into this binary; " + << "only options.num_linear_solver_threads=1 is supported. Switching" + << "to single threaded mode."; + options.num_linear_solver_threads = 1; + } +#endif + + // Reset the summary object to its default values; + *CHECK_NOTNULL(summary) = Solver::Summary(); + summary->linear_solver_type_given = options.linear_solver_type; + summary->num_eliminate_blocks_given = original_options.num_eliminate_blocks; + summary->num_threads_given = original_options.num_threads; + summary->num_linear_solver_threads_given = + original_options.num_linear_solver_threads; + summary->ordering_type = original_options.ordering_type; + + ProblemImpl* problem_impl = CHECK_NOTNULL(problem)->problem_impl_.get(); + + summary->num_parameter_blocks = problem_impl->NumParameterBlocks(); + summary->num_parameters = problem_impl->NumParameters(); + summary->num_residual_blocks = problem_impl->NumResidualBlocks(); + summary->num_residuals = problem_impl->NumResiduals(); + + summary->num_threads_used = options.num_threads; + + // Evaluate the initial cost and residual vector (if needed). The + // initial cost needs to be computed on the original unpreprocessed + // problem, as it is used to determine the value of the "fixed" part + // of the objective function after the problem has undergone + // reduction. Also the initial residuals are in the order in which + // the user added the ResidualBlocks to the optimization problem. + EvaluateCostAndResiduals(problem_impl, + &summary->initial_cost, + options.return_initial_residuals + ? &summary->initial_residuals + : NULL); + + // If the user requests gradient checking, construct a new + // ProblemImpl by wrapping the CostFunctions of problem_impl inside + // GradientCheckingCostFunction and replacing problem_impl with + // gradient_checking_problem_impl. + scoped_ptr<ProblemImpl> gradient_checking_problem_impl; + if (options.check_gradients) { + VLOG(1) << "Checking Gradients"; + gradient_checking_problem_impl.reset( + CreateGradientCheckingProblemImpl( + problem_impl, + options.numeric_derivative_relative_step_size, + options.gradient_check_relative_precision)); + + // From here on, problem_impl will point to the GradientChecking version. + problem_impl = gradient_checking_problem_impl.get(); + } + + // Create the three objects needed to minimize: the transformed program, the + // evaluator, and the linear solver. + + scoped_ptr<Program> reduced_program( + CreateReducedProgram(&options, problem_impl, &summary->error)); + if (reduced_program == NULL) { + return; + } + + summary->num_parameter_blocks_reduced = reduced_program->NumParameterBlocks(); + summary->num_parameters_reduced = reduced_program->NumParameters(); + summary->num_residual_blocks_reduced = reduced_program->NumResidualBlocks(); + summary->num_residuals_reduced = reduced_program->NumResiduals(); + + scoped_ptr<LinearSolver> + linear_solver(CreateLinearSolver(&options, &summary->error)); + summary->linear_solver_type_used = options.linear_solver_type; + summary->preconditioner_type = options.preconditioner_type; + summary->num_eliminate_blocks_used = options.num_eliminate_blocks; + summary->num_linear_solver_threads_used = options.num_linear_solver_threads; + + if (linear_solver == NULL) { + return; + } + + if (!MaybeReorderResidualBlocks(options, + reduced_program.get(), + &summary->error)) { + return; + } + + scoped_ptr<Evaluator> evaluator( + CreateEvaluator(options, reduced_program.get(), &summary->error)); + if (evaluator == NULL) { + return; + } + + // The optimizer works on contiguous parameter vectors; allocate some. + Vector initial_parameters(reduced_program->NumParameters()); + Vector optimized_parameters(reduced_program->NumParameters()); + + // Collect the discontiguous parameters into a contiguous state vector. + reduced_program->ParameterBlocksToStateVector(&initial_parameters[0]); + + // Run the optimization. + Minimize(options, + reduced_program.get(), + evaluator.get(), + linear_solver.get(), + initial_parameters.data(), + optimized_parameters.data(), + summary); + + // If the user aborted mid-optimization or the optimization + // terminated because of a numerical failure, then return without + // updating user state. + if (summary->termination_type == USER_ABORT || + summary->termination_type == NUMERICAL_FAILURE) { + return; + } + + // Push the contiguous optimized parameters back to the user's parameters. + reduced_program->StateVectorToParameterBlocks(&optimized_parameters[0]); + reduced_program->CopyParameterBlockStateToUserState(); + + // Return the final cost and residuals for the original problem. + EvaluateCostAndResiduals(problem->problem_impl_.get(), + &summary->final_cost, + options.return_final_residuals + ? &summary->final_residuals + : NULL); + + // Stick a fork in it, we're done. + return; +} + +// Strips varying parameters and residuals, maintaining order, and updating +// num_eliminate_blocks. +bool SolverImpl::RemoveFixedBlocksFromProgram(Program* program, + int* num_eliminate_blocks, + string* error) { + int original_num_eliminate_blocks = *num_eliminate_blocks; + vector<ParameterBlock*>* parameter_blocks = + program->mutable_parameter_blocks(); + + // Mark all the parameters as unused. Abuse the index member of the parameter + // blocks for the marking. + for (int i = 0; i < parameter_blocks->size(); ++i) { + (*parameter_blocks)[i]->set_index(-1); + } + + // Filter out residual that have all-constant parameters, and mark all the + // parameter blocks that appear in residuals. + { + vector<ResidualBlock*>* residual_blocks = + program->mutable_residual_blocks(); + int j = 0; + for (int i = 0; i < residual_blocks->size(); ++i) { + ResidualBlock* residual_block = (*residual_blocks)[i]; + int num_parameter_blocks = residual_block->NumParameterBlocks(); + + // Determine if the residual block is fixed, and also mark varying + // parameters that appear in the residual block. + bool all_constant = true; + for (int k = 0; k < num_parameter_blocks; k++) { + ParameterBlock* parameter_block = residual_block->parameter_blocks()[k]; + if (!parameter_block->IsConstant()) { + all_constant = false; + parameter_block->set_index(1); + } + } + + if (!all_constant) { + (*residual_blocks)[j++] = (*residual_blocks)[i]; + } + } + residual_blocks->resize(j); + } + + // Filter out unused or fixed parameter blocks, and update + // num_eliminate_blocks as necessary. + { + vector<ParameterBlock*>* parameter_blocks = + program->mutable_parameter_blocks(); + int j = 0; + for (int i = 0; i < parameter_blocks->size(); ++i) { + ParameterBlock* parameter_block = (*parameter_blocks)[i]; + if (parameter_block->index() == 1) { + (*parameter_blocks)[j++] = parameter_block; + } else if (i < original_num_eliminate_blocks) { + (*num_eliminate_blocks)--; + } + } + parameter_blocks->resize(j); + } + + CHECK(((program->NumResidualBlocks() == 0) && + (program->NumParameterBlocks() == 0)) || + ((program->NumResidualBlocks() != 0) && + (program->NumParameterBlocks() != 0))) + << "Congratulations, you found a bug in Ceres. Please report it."; + return true; +} + +Program* SolverImpl::CreateReducedProgram(Solver::Options* options, + ProblemImpl* problem_impl, + string* error) { + Program* original_program = problem_impl->mutable_program(); + scoped_ptr<Program> transformed_program(new Program(*original_program)); + + if (options->ordering_type == USER && + !ApplyUserOrdering(*problem_impl, + options->ordering, + transformed_program.get(), + error)) { + return NULL; + } + + if (options->ordering_type == SCHUR && options->num_eliminate_blocks != 0) { + *error = "Can't specify SCHUR ordering and num_eliminate_blocks " + "at the same time; SCHUR ordering determines " + "num_eliminate_blocks automatically."; + return NULL; + } + + if (options->ordering_type == SCHUR && options->ordering.size() != 0) { + *error = "Can't specify SCHUR ordering type and the ordering " + "vector at the same time; SCHUR ordering determines " + "a suitable parameter ordering automatically."; + return NULL; + } + + int num_eliminate_blocks = options->num_eliminate_blocks; + + if (!RemoveFixedBlocksFromProgram(transformed_program.get(), + &num_eliminate_blocks, + error)) { + return NULL; + } + + if (transformed_program->NumParameterBlocks() == 0) { + LOG(WARNING) << "No varying parameter blocks to optimize; " + << "bailing early."; + return transformed_program.release(); + } + + if (options->ordering_type == SCHUR) { + vector<ParameterBlock*> schur_ordering; + num_eliminate_blocks = ComputeSchurOrdering(*transformed_program, + &schur_ordering); + CHECK_EQ(schur_ordering.size(), transformed_program->NumParameterBlocks()) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + + // Replace the transformed program's ordering with the schur ordering. + swap(*transformed_program->mutable_parameter_blocks(), schur_ordering); + } + options->num_eliminate_blocks = num_eliminate_blocks; + CHECK_GE(options->num_eliminate_blocks, 0) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + + // Since the transformed program is the "active" program, and it is mutated, + // update the parameter offsets and indices. + transformed_program->SetParameterOffsetsAndIndex(); + return transformed_program.release(); +} + +LinearSolver* SolverImpl::CreateLinearSolver(Solver::Options* options, + string* error) { +#ifdef CERES_NO_SUITESPARSE + if (options->linear_solver_type == SPARSE_NORMAL_CHOLESKY) { + *error = "Can't use SPARSE_NORMAL_CHOLESKY because SuiteSparse was not " + "enabled when Ceres was built."; + return NULL; + } +#endif // CERES_NO_SUITESPARSE + + if (options->linear_solver_max_num_iterations <= 0) { + *error = "Solver::Options::linear_solver_max_num_iterations is 0."; + return NULL; + } + if (options->linear_solver_min_num_iterations <= 0) { + *error = "Solver::Options::linear_solver_min_num_iterations is 0."; + return NULL; + } + if (options->linear_solver_min_num_iterations > + options->linear_solver_max_num_iterations) { + *error = "Solver::Options::linear_solver_min_num_iterations > " + "Solver::Options::linear_solver_max_num_iterations."; + return NULL; + } + + LinearSolver::Options linear_solver_options; + linear_solver_options.constant_sparsity = true; + linear_solver_options.min_num_iterations = + options->linear_solver_min_num_iterations; + linear_solver_options.max_num_iterations = + options->linear_solver_max_num_iterations; + linear_solver_options.type = options->linear_solver_type; + linear_solver_options.preconditioner_type = options->preconditioner_type; + +#ifdef CERES_NO_SUITESPARSE + if (linear_solver_options.preconditioner_type == SCHUR_JACOBI) { + *error = "SCHUR_JACOBI preconditioner not suppored. Please build Ceres " + "with SuiteSparse support"; + return NULL; + } + + if (linear_solver_options.preconditioner_type == CLUSTER_JACOBI) { + *error = "CLUSTER_JACOBI preconditioner not suppored. Please build Ceres " + "with SuiteSparse support"; + return NULL; + } + + if (linear_solver_options.preconditioner_type == CLUSTER_TRIDIAGONAL) { + *error = "CLUSTER_TRIDIAGONAL preconditioner not suppored. Please build " + "Ceres with SuiteSparse support"; + return NULL; + } +#endif + + linear_solver_options.num_threads = options->num_linear_solver_threads; + linear_solver_options.num_eliminate_blocks = + options->num_eliminate_blocks; + + if ((linear_solver_options.num_eliminate_blocks == 0) && + IsSchurType(linear_solver_options.type)) { +#ifndef CERES_NO_SUITESPARSE + LOG(INFO) << "No elimination block remaining " + << "switching to SPARSE_NORMAL_CHOLESKY."; + linear_solver_options.type = SPARSE_NORMAL_CHOLESKY; +#else + LOG(INFO) << "No elimination block remaining switching to DENSE_QR."; + linear_solver_options.type = DENSE_QR; +#endif // CERES_NO_SUITESPARSE + } + +#ifdef CERES_NO_SUITESPARSE + if (linear_solver_options.type == SPARSE_SCHUR) { + *error = "Can't use SPARSE_SCHUR because SuiteSparse was not " + "enabled when Ceres was built."; + return NULL; + } +#endif // CERES_NO_SUITESPARSE + + // The matrix used for storing the dense Schur complement has a + // single lock guarding the whole matrix. Running the + // SchurComplementSolver with multiple threads leads to maximum + // contention and slowdown. If the problem is large enough to + // benefit from a multithreaded schur eliminator, you should be + // using a SPARSE_SCHUR solver anyways. + if ((linear_solver_options.num_threads > 1) && + (linear_solver_options.type == DENSE_SCHUR)) { + LOG(WARNING) << "Warning: Solver::Options::num_linear_solver_threads = " + << options->num_linear_solver_threads + << " with DENSE_SCHUR will result in poor performance; " + << "switching to single-threaded."; + linear_solver_options.num_threads = 1; + } + + options->linear_solver_type = linear_solver_options.type; + options->num_linear_solver_threads = linear_solver_options.num_threads; + + return LinearSolver::Create(linear_solver_options); +} + +bool SolverImpl::ApplyUserOrdering(const ProblemImpl& problem_impl, + vector<double*>& ordering, + Program* program, + string* error) { + if (ordering.size() != program->NumParameterBlocks()) { + *error = StringPrintf("User specified ordering does not have the same " + "number of parameters as the problem. The problem" + "has %d blocks while the ordering has %ld blocks.", + program->NumParameterBlocks(), + ordering.size()); + return false; + } + + // Ensure that there are no duplicates in the user's ordering. + { + vector<double*> ordering_copy(ordering); + sort(ordering_copy.begin(), ordering_copy.end()); + if (unique(ordering_copy.begin(), ordering_copy.end()) + != ordering_copy.end()) { + *error = "User specified ordering contains duplicates."; + return false; + } + } + + vector<ParameterBlock*>* parameter_blocks = + program->mutable_parameter_blocks(); + + fill(parameter_blocks->begin(), + parameter_blocks->end(), + static_cast<ParameterBlock*>(NULL)); + + const ProblemImpl::ParameterMap& parameter_map = problem_impl.parameter_map(); + for (int i = 0; i < ordering.size(); ++i) { + ProblemImpl::ParameterMap::const_iterator it = + parameter_map.find(ordering[i]); + if (it == parameter_map.end()) { + *error = StringPrintf("User specified ordering contains a pointer " + "to a double that is not a parameter block in the " + "problem. The invalid double is at position %d " + " in options.ordering.", i); + return false; + } + (*parameter_blocks)[i] = it->second; + } + return true; +} + +// Find the minimum index of any parameter block to the given residual. +// Parameter blocks that have indices greater than num_eliminate_blocks are +// considered to have an index equal to num_eliminate_blocks. +int MinParameterBlock(const ResidualBlock* residual_block, + int num_eliminate_blocks) { + int min_parameter_block_position = num_eliminate_blocks; + for (int i = 0; i < residual_block->NumParameterBlocks(); ++i) { + ParameterBlock* parameter_block = residual_block->parameter_blocks()[i]; + DCHECK_NE(parameter_block->index(), -1) + << "Did you forget to call Program::SetParameterOffsetsAndIndex()?"; + min_parameter_block_position = std::min(parameter_block->index(), + min_parameter_block_position); + } + return min_parameter_block_position; +} + +// Reorder the residuals for program, if necessary, so that the residuals +// involving each E block occur together. This is a necessary condition for the +// Schur eliminator, which works on these "row blocks" in the jacobian. +bool SolverImpl::MaybeReorderResidualBlocks(const Solver::Options& options, + Program* program, + string* error) { + // Only Schur types require the lexicographic reordering. + if (!IsSchurType(options.linear_solver_type)) { + return true; + } + + CHECK_NE(0, options.num_eliminate_blocks) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + + // Create a histogram of the number of residuals for each E block. There is an + // extra bucket at the end to catch all non-eliminated F blocks. + vector<int> residual_blocks_per_e_block(options.num_eliminate_blocks + 1); + vector<ResidualBlock*>* residual_blocks = program->mutable_residual_blocks(); + vector<int> min_position_per_residual(residual_blocks->size()); + for (int i = 0; i < residual_blocks->size(); ++i) { + ResidualBlock* residual_block = (*residual_blocks)[i]; + int position = MinParameterBlock(residual_block, + options.num_eliminate_blocks); + min_position_per_residual[i] = position; + DCHECK_LE(position, options.num_eliminate_blocks); + residual_blocks_per_e_block[position]++; + } + + // Run a cumulative sum on the histogram, to obtain offsets to the start of + // each histogram bucket (where each bucket is for the residuals for that + // E-block). + vector<int> offsets(options.num_eliminate_blocks + 1); + std::partial_sum(residual_blocks_per_e_block.begin(), + residual_blocks_per_e_block.end(), + offsets.begin()); + CHECK_EQ(offsets.back(), residual_blocks->size()) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + + CHECK(find(residual_blocks_per_e_block.begin(), + residual_blocks_per_e_block.end() - 1, 0) != + residual_blocks_per_e_block.end()) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + + // Fill in each bucket with the residual blocks for its corresponding E block. + // Each bucket is individually filled from the back of the bucket to the front + // of the bucket. The filling order among the buckets is dictated by the + // residual blocks. This loop uses the offsets as counters; subtracting one + // from each offset as a residual block is placed in the bucket. When the + // filling is finished, the offset pointerts should have shifted down one + // entry (this is verified below). + vector<ResidualBlock*> reordered_residual_blocks( + (*residual_blocks).size(), static_cast<ResidualBlock*>(NULL)); + for (int i = 0; i < residual_blocks->size(); ++i) { + int bucket = min_position_per_residual[i]; + + // Decrement the cursor, which should now point at the next empty position. + offsets[bucket]--; + + // Sanity. + CHECK(reordered_residual_blocks[offsets[bucket]] == NULL) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + + reordered_residual_blocks[offsets[bucket]] = (*residual_blocks)[i]; + } + + // Sanity check #1: The difference in bucket offsets should match the + // histogram sizes. + for (int i = 0; i < options.num_eliminate_blocks; ++i) { + CHECK_EQ(residual_blocks_per_e_block[i], offsets[i + 1] - offsets[i]) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + } + // Sanity check #2: No NULL's left behind. + for (int i = 0; i < reordered_residual_blocks.size(); ++i) { + CHECK(reordered_residual_blocks[i] != NULL) + << "Congratulations, you found a Ceres bug! Please report this error " + << "to the developers."; + } + + // Now that the residuals are collected by E block, swap them in place. + swap(*program->mutable_residual_blocks(), reordered_residual_blocks); + return true; +} + +Evaluator* SolverImpl::CreateEvaluator(const Solver::Options& options, + Program* program, + string* error) { + Evaluator::Options evaluator_options; + evaluator_options.linear_solver_type = options.linear_solver_type; + evaluator_options.num_eliminate_blocks = options.num_eliminate_blocks; + evaluator_options.num_threads = options.num_threads; + return Evaluator::Create(evaluator_options, program, error); +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/solver_impl.h b/extern/libmv/third_party/ceres/internal/ceres/solver_impl.h new file mode 100644 index 00000000000..957ebcc65df --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/solver_impl.h @@ -0,0 +1,111 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#ifndef CERES_INTERNAL_SOLVER_IMPL_H_ +#define CERES_INTERNAL_SOLVER_IMPL_H_ + +#include "ceres/solver.h" + +namespace ceres { +namespace internal { + +class Evaluator; +class LinearSolver; +class ProblemImpl; +class Program; + +class SolverImpl { + public: + // Mirrors the interface in solver.h, but exposes implementation + // details for testing internally. + static void Solve(const Solver::Options& options, + Problem* problem, + Solver::Summary* summary); + + // Create the transformed Program, which has all the fixed blocks + // and residuals eliminated, and in the case of automatic schur + // ordering, has the E blocks first in the resulting program, with + // options.num_eliminate_blocks set appropriately. + static Program* CreateReducedProgram(Solver::Options* options, + ProblemImpl* problem_impl, + string* error); + + // Create the appropriate linear solver, taking into account any + // config changes decided by CreateTransformedProgram(). The + // selected linear solver, which may be different from what the user + // selected; consider the case that the remaining elimininated + // blocks is zero after removing fixed blocks. + static LinearSolver* CreateLinearSolver(Solver::Options* options, + string* error); + + // Reorder the parameter blocks in program using the vector + // ordering. A return value of true indicates success and false + // indicates an error was encountered whose cause is logged to + // LOG(ERROR). + static bool ApplyUserOrdering(const ProblemImpl& problem_impl, + vector<double*>& ordering, + Program* program, + string* error); + + // Reorder the residuals for program, if necessary, so that the + // residuals involving each E block occur together. This is a + // necessary condition for the Schur eliminator, which works on + // these "row blocks" in the jacobian. + static bool MaybeReorderResidualBlocks(const Solver::Options& options, + Program* program, + string* error); + + // Create the appropriate evaluator for the transformed program. + static Evaluator* CreateEvaluator(const Solver::Options& options, + Program* program, + string* error); + + // Run the minimization for the given evaluator and configuration. + static void Minimize(const Solver::Options &options, + Program* program, + Evaluator* evaluator, + LinearSolver* linear_solver, + double* initial_parameters, + double* final_parameters, + Solver::Summary* summary); + + // Remove the fixed or unused parameter blocks and residuals + // depending only on fixed parameters from the problem. Also updates + // num_eliminate_blocks, since removed parameters changes the point + // at which the eliminated blocks is valid. + static bool RemoveFixedBlocksFromProgram(Program* program, + int* num_eliminate_blocks, + string* error); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_SOLVER_IMPL_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/sparse_matrix.cc b/extern/libmv/third_party/ceres/internal/ceres/sparse_matrix.cc new file mode 100644 index 00000000000..55336fd3130 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/sparse_matrix.cc @@ -0,0 +1,40 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/sparse_matrix.h" + +namespace ceres { +namespace internal { + +SparseMatrix::~SparseMatrix() { +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/sparse_matrix.h b/extern/libmv/third_party/ceres/internal/ceres/sparse_matrix.h new file mode 100644 index 00000000000..562210dfec8 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/sparse_matrix.h @@ -0,0 +1,114 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Interface definition for sparse matrices. + +#ifndef CERES_INTERNAL_SPARSE_MATRIX_H_ +#define CERES_INTERNAL_SPARSE_MATRIX_H_ + +#include <cstdio> +#include "ceres/linear_operator.h" +#include "ceres/internal/eigen.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +class SparseMatrixProto; + +// This class defines the interface for storing and manipulating +// sparse matrices. The key property that differentiates different +// sparse matrices is how they are organized in memory and how the +// information about the sparsity structure of the matrix is +// stored. This has significant implications for linear solvers +// operating on these matrices. +// +// To deal with the different kinds of layouts, we will assume that a +// sparse matrix will have a two part representation. A values array +// that will be used to store the entries of the sparse matrix and +// some sort of a layout object that tells the user the sparsity +// structure and layout of the values array. For example in case of +// the TripletSparseMatrix, this information is carried in the rows +// and cols arrays and for the BlockSparseMatrix, this information is +// carried in the CompressedRowBlockStructure object. +// +// This interface deliberately does not contain any information about +// the structure of the sparse matrix as that seems to be highly +// matrix type dependent and we are at this stage unable to come up +// with an efficient high level interface that spans multiple sparse +// matrix types. +class SparseMatrix : public LinearOperator { + public: + virtual ~SparseMatrix(); + + // y += Ax; + virtual void RightMultiply(const double* x, double* y) const = 0; + // y += A'x; + virtual void LeftMultiply(const double* x, double* y) const = 0; + + // In MATLAB notation sum(A.*A, 1) + virtual void SquaredColumnNorm(double* x) const = 0; + // A = A * diag(scale) + virtual void ScaleColumns(const double* scale) = 0; + + // A = 0. A->num_nonzeros() == 0 is true after this call. The + // sparsity pattern is preserved. + virtual void SetZero() = 0; + + // Resize and populate dense_matrix with a dense version of the + // sparse matrix. + virtual void ToDenseMatrix(Matrix* dense_matrix) const = 0; + +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS + // Dump the sparse matrix to a proto. Destroys the contents of proto. + virtual void ToProto(SparseMatrixProto* proto) const = 0; +#endif + + // Write out the matrix as a sequence of (i,j,s) triplets. This + // format is useful for loading the matrix into MATLAB/octave as a + // sparse matrix. + virtual void ToTextFile(FILE* file) const = 0; + + // Accessors for the values array that stores the entries of the + // sparse matrix. The exact interpreptation of the values of this + // array depends on the particular kind of SparseMatrix being + // accessed. + virtual double* mutable_values() = 0; + virtual const double* values() const = 0; + + virtual int num_rows() const = 0; + virtual int num_cols() const = 0; + virtual int num_nonzeros() const = 0; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_SPARSE_MATRIX_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/sparse_normal_cholesky_solver.cc b/extern/libmv/third_party/ceres/internal/ceres/sparse_normal_cholesky_solver.cc new file mode 100644 index 00000000000..59222dc374d --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/sparse_normal_cholesky_solver.cc @@ -0,0 +1,129 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_NO_SUITESPARSE + +#include "ceres/sparse_normal_cholesky_solver.h" + +#include <algorithm> +#include <cstring> +#include <ctime> +#include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/linear_solver.h" +#include "ceres/suitesparse.h" +#include "ceres/triplet_sparse_matrix.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +SparseNormalCholeskySolver::SparseNormalCholeskySolver( + const LinearSolver::Options& options) + : options_(options), symbolic_factor_(NULL) {} + +SparseNormalCholeskySolver::~SparseNormalCholeskySolver() { + if (symbolic_factor_ != NULL) { + ss_.Free(symbolic_factor_); + symbolic_factor_ = NULL; + } +} + +LinearSolver::Summary SparseNormalCholeskySolver::SolveImpl( + CompressedRowSparseMatrix* A, + const double* b, + const LinearSolver::PerSolveOptions& per_solve_options, + double * x) { + const time_t start_time = time(NULL); + const int num_cols = A->num_cols(); + + LinearSolver::Summary summary; + Vector Atb = Vector::Zero(num_cols); + A->LeftMultiply(b, Atb.data()); + + if (per_solve_options.D != NULL) { + // Temporarily append a diagonal block to the A matrix, but undo it before + // returning the matrix to the user. + CompressedRowSparseMatrix D(per_solve_options.D, num_cols); + A->AppendRows(D); + } + + VectorRef(x, num_cols).setZero(); + + scoped_ptr<cholmod_sparse> lhs(ss_.CreateSparseMatrixTransposeView(A)); + CHECK_NOTNULL(lhs.get()); + + cholmod_dense* rhs = ss_.CreateDenseVector(Atb.data(), num_cols, num_cols); + const time_t init_time = time(NULL); + + if (symbolic_factor_ == NULL) { + symbolic_factor_ = CHECK_NOTNULL(ss_.AnalyzeCholesky(lhs.get())); + } + + const time_t symbolic_time = time(NULL); + + cholmod_dense* sol = ss_.SolveCholesky(lhs.get(), symbolic_factor_, rhs); + const time_t solve_time = time(NULL); + + ss_.Free(rhs); + rhs = NULL; + + if (per_solve_options.D != NULL) { + A->DeleteRows(num_cols); + } + + if (!options_.constant_sparsity) { + ss_.Free(symbolic_factor_); + symbolic_factor_ = NULL; + } + + summary.num_iterations = 1; + if (sol != NULL) { + memcpy(x, sol->x, num_cols * sizeof(*x)); + + ss_.Free(sol); + sol = NULL; + summary.termination_type = TOLERANCE; + } + + const time_t cleanup_time = time(NULL); + VLOG(2) << "time (sec) total: " << cleanup_time - start_time + << " init: " << init_time - start_time + << " symbolic: " << symbolic_time - init_time + << " solve: " << solve_time - symbolic_time + << " cleanup: " << cleanup_time - solve_time; + return summary; +} + +} // namespace internal +} // namespace ceres + +#endif // CERES_NO_SUITESPARSE diff --git a/extern/libmv/third_party/ceres/internal/ceres/sparse_normal_cholesky_solver.h b/extern/libmv/third_party/ceres/internal/ceres/sparse_normal_cholesky_solver.h new file mode 100644 index 00000000000..ce1d6d285be --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/sparse_normal_cholesky_solver.h @@ -0,0 +1,77 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// A solver for sparse linear least squares problem based on solving +// the normal equations via a sparse cholesky factorization. + +#ifndef CERES_INTERNAL_SPARSE_NORMAL_CHOLESKY_SOLVER_H_ +#define CERES_INTERNAL_SPARSE_NORMAL_CHOLESKY_SOLVER_H_ + +#ifndef CERES_NO_SUITESPARSE + +#include "cholmod.h" +#include "cholmod_core.h" +#include "ceres/linear_solver.h" +#include "ceres/suitesparse.h" +#include "ceres/internal/macros.h" + +namespace ceres { +namespace internal { + +class CompressedRowSparseMatrix; + +// Solves the normal equations (A'A + D'D) x = A'b, using the CHOLMOD sparse +// cholesky solver. +class SparseNormalCholeskySolver : public CompressedRowSparseMatrixSolver { + public: + explicit SparseNormalCholeskySolver(const LinearSolver::Options& options); + virtual ~SparseNormalCholeskySolver(); + + private: + virtual LinearSolver::Summary SolveImpl( + CompressedRowSparseMatrix* A, + const double* b, + const LinearSolver::PerSolveOptions& options, + double* x); + + const LinearSolver::Options options_; + SuiteSparse ss_; + + // Cached factorization + cholmod_factor* symbolic_factor_; + DISALLOW_COPY_AND_ASSIGN(SparseNormalCholeskySolver); +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_NO_SUITESPARSE + +#endif // CERES_INTERNAL_SPARSE_NORMAL_CHOLESKY_SOLVER_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/split.cc b/extern/libmv/third_party/ceres/internal/ceres/split.cc new file mode 100644 index 00000000000..4fa1bd468b9 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/split.cc @@ -0,0 +1,115 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#include <string> +#include <vector> +#include <iterator> +#include "ceres/internal/port.h" + +namespace ceres { + +// If we know how much to allocate for a vector of strings, we can allocate the +// vector<string> only once and directly to the right size. This saves in +// between 33-66 % of memory space needed for the result, and runs faster in the +// microbenchmarks. +// +// The reserve is only implemented for the single character delim. +// +// The implementation for counting is cut-and-pasted from +// SplitStringToIteratorUsing. I could have written my own counting iterator, +// and use the existing template function, but probably this is more clear and +// more sure to get optimized to reasonable code. +static int CalculateReserveForVector(const string& full, const char* delim) { + int count = 0; + if (delim[0] != '\0' && delim[1] == '\0') { + // Optimize the common case where delim is a single character. + char c = delim[0]; + const char* p = full.data(); + const char* end = p + full.size(); + while (p != end) { + if (*p == c) { // This could be optimized with hasless(v,1) trick. + ++p; + } else { + while (++p != end && *p != c) { + // Skip to the next occurence of the delimiter. + } + ++count; + } + } + } + return count; +} + +template <typename StringType, typename ITR> +static inline +void SplitStringToIteratorUsing(const StringType& full, + const char* delim, + ITR& result) { + // Optimize the common case where delim is a single character. + if (delim[0] != '\0' && delim[1] == '\0') { + char c = delim[0]; + const char* p = full.data(); + const char* end = p + full.size(); + while (p != end) { + if (*p == c) { + ++p; + } else { + const char* start = p; + while (++p != end && *p != c) { + // Skip to the next occurence of the delimiter. + } + *result++ = StringType(start, p - start); + } + } + return; + } + + string::size_type begin_index, end_index; + begin_index = full.find_first_not_of(delim); + while (begin_index != string::npos) { + end_index = full.find_first_of(delim, begin_index); + if (end_index == string::npos) { + *result++ = full.substr(begin_index); + return; + } + *result++ = full.substr(begin_index, (end_index - begin_index)); + begin_index = full.find_first_not_of(delim, end_index); + } +} + +void SplitStringUsing(const string& full, + const char* delim, + vector<string>* result) { + result->reserve(result->size() + CalculateReserveForVector(full, delim)); + back_insert_iterator< vector<string> > it(*result); + SplitStringToIteratorUsing(full, delim, it); +} + +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/stl_util.h b/extern/libmv/third_party/ceres/internal/ceres/stl_util.h new file mode 100644 index 00000000000..a1a19e8b3ce --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/stl_util.h @@ -0,0 +1,75 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: keir@google.com (Keir Mierle) + +#ifndef CERES_INTERNAL_STL_UTIL_H_ +#define CERES_INTERNAL_STL_UTIL_H_ + +namespace ceres { + +// STLDeleteContainerPointers() +// For a range within a container of pointers, calls delete +// (non-array version) on these pointers. +// NOTE: for these three functions, we could just implement a DeleteObject +// functor and then call for_each() on the range and functor, but this +// requires us to pull in all of algorithm.h, which seems expensive. +// For hash_[multi]set, it is important that this deletes behind the iterator +// because the hash_set may call the hash function on the iterator when it is +// advanced, which could result in the hash function trying to deference a +// stale pointer. +template <class ForwardIterator> +void STLDeleteContainerPointers(ForwardIterator begin, + ForwardIterator end) { + while (begin != end) { + ForwardIterator temp = begin; + ++begin; + delete *temp; + } +} + +// STLDeleteElements() deletes all the elements in an STL container and clears +// the container. This function is suitable for use with a vector, set, +// hash_set, or any other STL container which defines sensible begin(), end(), +// and clear() methods. +// +// If container is NULL, this function is a no-op. +// +// As an alternative to calling STLDeleteElements() directly, consider +// ElementDeleter (defined below), which ensures that your container's elements +// are deleted when the ElementDeleter goes out of scope. +template <class T> +void STLDeleteElements(T *container) { + if (!container) return; + STLDeleteContainerPointers(container->begin(), container->end()); + container->clear(); +} + +} // namespace ceres + +#endif // CERES_INTERNAL_STL_UTIL_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/stringprintf.cc b/extern/libmv/third_party/ceres/internal/ceres/stringprintf.cc new file mode 100644 index 00000000000..c0f35225bc3 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/stringprintf.cc @@ -0,0 +1,126 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: Sanjay Ghemawat + +#include <cerrno> +#include <cstdarg> // For va_list and related operations +#include <cstdio> // MSVC requires this for _vsnprintf +#include <string> +#include <vector> + +#include "ceres/internal/port.h" + +namespace ceres { +namespace internal { + +#ifdef _MSC_VER +enum { IS_COMPILER_MSVC = 1 }; +#define va_copy(d,s) ((d) = (s)) +#else +enum { IS_COMPILER_MSVC = 0 }; +#endif + +void StringAppendV(string* dst, const char* format, va_list ap) { + // First try with a small fixed size buffer + char space[1024]; + + // It's possible for methods that use a va_list to invalidate + // the data in it upon use. The fix is to make a copy + // of the structure before using it and use that copy instead. + va_list backup_ap; + va_copy(backup_ap, ap); + int result = vsnprintf(space, sizeof(space), format, backup_ap); + va_end(backup_ap); + + if (result < sizeof(space)) { + if (result >= 0) { + // Normal case -- everything fit. + dst->append(space, result); + return; + } + + if (IS_COMPILER_MSVC) { + // Error or MSVC running out of space. MSVC 8.0 and higher + // can be asked about space needed with the special idiom below: + va_copy(backup_ap, ap); + result = vsnprintf(NULL, 0, format, backup_ap); + va_end(backup_ap); + } + + if (result < 0) { + // Just an error. + return; + } + } + + // Increase the buffer size to the size requested by vsnprintf, + // plus one for the closing \0. + int length = result+1; + char* buf = new char[length]; + + // Restore the va_list before we use it again + va_copy(backup_ap, ap); + result = vsnprintf(buf, length, format, backup_ap); + va_end(backup_ap); + + if (result >= 0 && result < length) { + // It fit + dst->append(buf, result); + } + delete[] buf; +} + + +string StringPrintf(const char* format, ...) { + va_list ap; + va_start(ap, format); + string result; + StringAppendV(&result, format, ap); + va_end(ap); + return result; +} + +const string& SStringPrintf(string* dst, const char* format, ...) { + va_list ap; + va_start(ap, format); + dst->clear(); + StringAppendV(dst, format, ap); + va_end(ap); + return *dst; +} + +void StringAppendF(string* dst, const char* format, ...) { + va_list ap; + va_start(ap, format); + StringAppendV(dst, format, ap); + va_end(ap); +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/stringprintf.h b/extern/libmv/third_party/ceres/internal/ceres/stringprintf.h new file mode 100644 index 00000000000..30b974e7ae5 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/stringprintf.h @@ -0,0 +1,89 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: Sanjay Ghemawat +// +// Printf variants that place their output in a C++ string. +// +// Usage: +// string result = StringPrintf("%d %s\n", 10, "hello"); +// SStringPrintf(&result, "%d %s\n", 10, "hello"); +// StringAppendF(&result, "%d %s\n", 20, "there"); + +#ifndef CERES_INTERNAL_STRINGPRINTF_H_ +#define CERES_INTERNAL_STRINGPRINTF_H_ + +#include <cstdarg> +#include <string> + +#include "ceres/internal/port.h" + +namespace ceres { +namespace internal { + +#if (defined(__GNUC__) || defined(__clang__)) +// Tell the compiler to do printf format string checking if the compiler +// supports it; see the 'format' attribute in +// <http://gcc.gnu.org/onlinedocs/gcc-4.3.0/gcc/Function-Attributes.html>. +// +// N.B.: As the GCC manual states, "[s]ince non-static C++ methods +// have an implicit 'this' argument, the arguments of such methods +// should be counted from two, not one." +#define PRINTF_ATTRIBUTE(string_index, first_to_check) \ + __attribute__((__format__ (__printf__, string_index, first_to_check))) +#define SCANF_ATTRIBUTE(string_index, first_to_check) \ + __attribute__((__format__ (__scanf__, string_index, first_to_check))) +#else +#define PRINTF_ATTRIBUTE(string_index, first_to_check) +#endif + +// Return a C++ string. +extern string StringPrintf(const char* format, ...) + // Tell the compiler to do printf format string checking. + PRINTF_ATTRIBUTE(1,2); + +// Store result into a supplied string and return it. +extern const string& SStringPrintf(string* dst, const char* format, ...) + // Tell the compiler to do printf format string checking. + PRINTF_ATTRIBUTE(2,3); + +// Append result to a supplied string. +extern void StringAppendF(string* dst, const char* format, ...) + // Tell the compiler to do printf format string checking. + PRINTF_ATTRIBUTE(2,3); + +// Lower-level routine that takes a va_list and appends to a specified string. +// All other routines are just convenience wrappers around it. +extern void StringAppendV(string* dst, const char* format, va_list ap); + +#undef PRINTF_ATTRIBUTE + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_STRINGPRINTF_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/suitesparse.cc b/extern/libmv/third_party/ceres/internal/ceres/suitesparse.cc new file mode 100644 index 00000000000..1cf6a7496a7 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/suitesparse.cc @@ -0,0 +1,193 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_NO_SUITESPARSE + +#include "ceres/suitesparse.h" + +#include "cholmod.h" +#include "ceres/compressed_row_sparse_matrix.h" +#include "ceres/triplet_sparse_matrix.h" +namespace ceres { +namespace internal { + +cholmod_sparse* SuiteSparse::CreateSparseMatrix(TripletSparseMatrix* A) { + cholmod_triplet triplet; + + triplet.nrow = A->num_rows(); + triplet.ncol = A->num_cols(); + triplet.nzmax = A->max_num_nonzeros(); + triplet.nnz = A->num_nonzeros(); + triplet.i = reinterpret_cast<void*>(A->mutable_rows()); + triplet.j = reinterpret_cast<void*>(A->mutable_cols()); + triplet.x = reinterpret_cast<void*>(A->mutable_values()); + triplet.stype = 0; // Matrix is not symmetric. + triplet.itype = CHOLMOD_INT; + triplet.xtype = CHOLMOD_REAL; + triplet.dtype = CHOLMOD_DOUBLE; + + return cholmod_triplet_to_sparse(&triplet, triplet.nnz, &cc_); +} + + +cholmod_sparse* SuiteSparse::CreateSparseMatrixTranspose( + TripletSparseMatrix* A) { + cholmod_triplet triplet; + + triplet.ncol = A->num_rows(); // swap row and columns + triplet.nrow = A->num_cols(); + triplet.nzmax = A->max_num_nonzeros(); + triplet.nnz = A->num_nonzeros(); + + // swap rows and columns + triplet.j = reinterpret_cast<void*>(A->mutable_rows()); + triplet.i = reinterpret_cast<void*>(A->mutable_cols()); + triplet.x = reinterpret_cast<void*>(A->mutable_values()); + triplet.stype = 0; // Matrix is not symmetric. + triplet.itype = CHOLMOD_INT; + triplet.xtype = CHOLMOD_REAL; + triplet.dtype = CHOLMOD_DOUBLE; + + return cholmod_triplet_to_sparse(&triplet, triplet.nnz, &cc_); +} + +cholmod_sparse* SuiteSparse::CreateSparseMatrixTransposeView( + CompressedRowSparseMatrix* A) { + cholmod_sparse* m = new cholmod_sparse_struct; + m->nrow = A->num_cols(); + m->ncol = A->num_rows(); + m->nzmax = A->num_nonzeros(); + + m->p = reinterpret_cast<void*>(A->mutable_rows()); + m->i = reinterpret_cast<void*>(A->mutable_cols()); + m->x = reinterpret_cast<void*>(A->mutable_values()); + + m->stype = 0; // Matrix is not symmetric. + m->itype = CHOLMOD_INT; + m->xtype = CHOLMOD_REAL; + m->dtype = CHOLMOD_DOUBLE; + m->sorted = 1; + m->packed = 1; + + return m; +} + +cholmod_dense* SuiteSparse::CreateDenseVector(const double* x, + int in_size, + int out_size) { + CHECK_LE(in_size, out_size); + cholmod_dense* v = cholmod_zeros(out_size, 1, CHOLMOD_REAL, &cc_); + if (x != NULL) { + memcpy(v->x, x, in_size*sizeof(*x)); + } + return v; +} + +cholmod_factor* SuiteSparse::AnalyzeCholesky(cholmod_sparse* A) { + cholmod_factor* factor = cholmod_analyze(A, &cc_); + CHECK_EQ(cc_.status, CHOLMOD_OK) + << "Cholmod symbolic analysis failed " << cc_.status; + CHECK_NOTNULL(factor); + return factor; +} + +bool SuiteSparse::Cholesky(cholmod_sparse* A, cholmod_factor* L) { + CHECK_NOTNULL(A); + CHECK_NOTNULL(L); + + cc_.quick_return_if_not_posdef = 1; + int status = cholmod_factorize(A, L, &cc_); + switch (cc_.status) { + case CHOLMOD_NOT_INSTALLED: + LOG(WARNING) << "Cholmod failure: method not installed."; + return false; + case CHOLMOD_OUT_OF_MEMORY: + LOG(WARNING) << "Cholmod failure: out of memory."; + return false; + case CHOLMOD_TOO_LARGE: + LOG(WARNING) << "Cholmod failure: integer overflow occured."; + return false; + case CHOLMOD_INVALID: + LOG(WARNING) << "Cholmod failure: invalid input."; + return false; + case CHOLMOD_NOT_POSDEF: + // TODO(sameeragarwal): These two warnings require more + // sophisticated handling going forward. For now we will be + // strict and treat them as failures. + LOG(WARNING) << "Cholmod warning: matrix not positive definite."; + return false; + case CHOLMOD_DSMALL: + LOG(WARNING) << "Cholmod warning: D for LDL' or diag(L) or " + << "LL' has tiny absolute value."; + return false; + case CHOLMOD_OK: + if (status != 0) { + return true; + } + LOG(WARNING) << "Cholmod failure: cholmod_factorize returned zero " + << "but cholmod_common::status is CHOLMOD_OK." + << "Please report this to ceres-solver@googlegroups.com."; + return false; + default: + LOG(WARNING) << "Unknown cholmod return code. " + << "Please report this to ceres-solver@googlegroups.com."; + return false; + } + return false; +} + +cholmod_dense* SuiteSparse::Solve(cholmod_factor* L, + cholmod_dense* b) { + if (cc_.status != CHOLMOD_OK) { + LOG(WARNING) << "CHOLMOD status NOT OK"; + return NULL; + } + + return cholmod_solve(CHOLMOD_A, L, b, &cc_); +} + +cholmod_dense* SuiteSparse::SolveCholesky(cholmod_sparse* A, + cholmod_factor* L, + cholmod_dense* b) { + CHECK_NOTNULL(A); + CHECK_NOTNULL(L); + CHECK_NOTNULL(b); + + if (Cholesky(A, L)) { + return Solve(L, b); + } + + return NULL; +} + +} // namespace internal +} // namespace ceres + +#endif // CERES_NO_SUITESPARSE diff --git a/extern/libmv/third_party/ceres/internal/ceres/suitesparse.h b/extern/libmv/third_party/ceres/internal/ceres/suitesparse.h new file mode 100644 index 00000000000..091e67a69a9 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/suitesparse.h @@ -0,0 +1,159 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// A simple C++ interface to the SuiteSparse and CHOLMOD libraries. + +#ifndef CERES_INTERNAL_SUITESPARSE_H_ +#define CERES_INTERNAL_SUITESPARSE_H_ + +#ifndef CERES_NO_SUITESPARSE + +#include <cstring> +#include <string> + +#include <glog/logging.h> +#include "cholmod.h" +#include "ceres/internal/port.h" + +namespace ceres { +namespace internal { + +class CompressedRowSparseMatrix; +class TripletSparseMatrix; + +// The raw CHOLMOD and SuiteSparseQR libraries have a slightly +// cumbersome c like calling format. This object abstracts it away and +// provides the user with a simpler interface. The methods here cannot +// be static as a cholmod_common object serves as a global variable +// for all cholmod function calls. +class SuiteSparse { + public: + SuiteSparse() { cholmod_start(&cc_); } + ~SuiteSparse() { cholmod_finish(&cc_); } + + // Functions for building cholmod_sparse objects from sparse + // matrices stored in triplet form. The matrix A is not + // modifed. Called owns the result. + cholmod_sparse* CreateSparseMatrix(TripletSparseMatrix* A); + + // This function works like CreateSparseMatrix, except that the + // return value corresponds to A' rather than A. + cholmod_sparse* CreateSparseMatrixTranspose(TripletSparseMatrix* A); + + // Create a cholmod_sparse wrapper around the contents of A. This is + // a shallow object, which refers to the contents of A and does not + // use the SuiteSparse machinery to allocate memory, this object + // should be disposed off with a delete and not a call to Free as is + // the case for objects returned by CreateSparseMatrixTranspose. + cholmod_sparse* CreateSparseMatrixTransposeView(CompressedRowSparseMatrix* A); + + // Given a vector x, build a cholmod_dense vector of size out_size + // with the first in_size entries copied from x. If x is NULL, then + // an all zeros vector is returned. Caller owns the result. + cholmod_dense* CreateDenseVector(const double* x, int in_size, int out_size); + + // The matrix A is scaled using the matrix whose diagonal is the + // vector scale. mode describes how scaling is applied. Possible + // values are CHOLMOD_ROW for row scaling - diag(scale) * A, + // CHOLMOD_COL for column scaling - A * diag(scale) and CHOLMOD_SYM + // for symmetric scaling which scales both the rows and the columns + // - diag(scale) * A * diag(scale). + void Scale(cholmod_dense* scale, int mode, cholmod_sparse* A) { + cholmod_scale(scale, mode, A, &cc_); + } + + // Create and return a matrix m = A * A'. Caller owns the + // result. The matrix A is not modified. + cholmod_sparse* AATranspose(cholmod_sparse* A) { + cholmod_sparse*m = cholmod_aat(A, NULL, A->nrow, 1, &cc_); + m->stype = 1; // Pay attention to the upper triangular part. + return m; + } + + // y = alpha * A * x + beta * y. Only y is modified. + void SparseDenseMultiply(cholmod_sparse* A, double alpha, double beta, + cholmod_dense* x, cholmod_dense* y) { + double alpha_[2] = {alpha, 0}; + double beta_[2] = {beta, 0}; + cholmod_sdmult(A, 0, alpha_, beta_, x, y, &cc_); + } + + // Analyze the sparsity structure of the matrix A compute the + // symbolic factorization of A. A is not modified, only the pattern + // of non-zeros of A is used, the actual numerical values in A are + // of no consequence. Caller owns the result. + cholmod_factor* AnalyzeCholesky(cholmod_sparse* A); + + // Use the symbolic factorization in L, to find the numerical + // factorization for the matrix A or AA^T. Return true if + // successful, false otherwise. L contains the numeric factorization + // on return. + bool Cholesky(cholmod_sparse* A, cholmod_factor* L); + + // Given a Cholesky factorization of a matrix A = LL^T, solve the + // linear system Ax = b, and return the result. If the Solve fails + // NULL is returned. Caller owns the result. + cholmod_dense* Solve(cholmod_factor* L, cholmod_dense* b); + + // Combine the calls to Cholesky and Solve into a single call. If + // the cholesky factorization or the solve fails, return + // NULL. Caller owns the result. + cholmod_dense* SolveCholesky(cholmod_sparse* A, + cholmod_factor* L, + cholmod_dense* b); + + void Free(cholmod_sparse* m) { cholmod_free_sparse(&m, &cc_); } + void Free(cholmod_dense* m) { cholmod_free_dense(&m, &cc_); } + void Free(cholmod_factor* m) { cholmod_free_factor(&m, &cc_); } + + void Print(cholmod_sparse* m, const string& name) { + cholmod_print_sparse(m, const_cast<char*>(name.c_str()), &cc_); + } + + void Print(cholmod_dense* m, const string& name) { + cholmod_print_dense(m, const_cast<char*>(name.c_str()), &cc_); + } + + void Print(cholmod_triplet* m, const string& name) { + cholmod_print_triplet(m, const_cast<char*>(name.c_str()), &cc_); + } + + cholmod_common* mutable_cc() { return &cc_; } + + private: + cholmod_common cc_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_NO_SUITESPARSE + +#endif // CERES_INTERNAL_SUITESPARSE_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/triplet_sparse_matrix.cc b/extern/libmv/third_party/ceres/internal/ceres/triplet_sparse_matrix.cc new file mode 100644 index 00000000000..247ab2e697b --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/triplet_sparse_matrix.cc @@ -0,0 +1,306 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/triplet_sparse_matrix.h" + +#include <algorithm> +#include <cstddef> +#include <glog/logging.h> +#include "ceres/matrix_proto.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/port.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +TripletSparseMatrix::TripletSparseMatrix() + : num_rows_(0), + num_cols_(0), + max_num_nonzeros_(0), + num_nonzeros_(0), + rows_(NULL), + cols_(NULL), + values_(NULL) {} + +TripletSparseMatrix::~TripletSparseMatrix() {} + +TripletSparseMatrix::TripletSparseMatrix(int num_rows, + int num_cols, + int max_num_nonzeros) + : num_rows_(num_rows), + num_cols_(num_cols), + max_num_nonzeros_(max_num_nonzeros), + num_nonzeros_(0), + rows_(NULL), + cols_(NULL), + values_(NULL) { + // All the sizes should at least be zero + CHECK_GE(num_rows, 0); + CHECK_GE(num_cols, 0); + CHECK_GE(max_num_nonzeros, 0); + AllocateMemory(); +} + +TripletSparseMatrix::TripletSparseMatrix(const TripletSparseMatrix& orig) + : num_rows_(orig.num_rows_), + num_cols_(orig.num_cols_), + max_num_nonzeros_(orig.max_num_nonzeros_), + num_nonzeros_(orig.num_nonzeros_), + rows_(NULL), + cols_(NULL), + values_(NULL) { + AllocateMemory(); + CopyData(orig); +} + +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +TripletSparseMatrix::TripletSparseMatrix(const SparseMatrixProto& outer_proto) { + CHECK(outer_proto.has_triplet_matrix()); + + const TripletSparseMatrixProto& proto = outer_proto.triplet_matrix(); + CHECK(proto.has_num_rows()); + CHECK(proto.has_num_cols()); + CHECK_EQ(proto.rows_size(), proto.cols_size()); + CHECK_EQ(proto.cols_size(), proto.values_size()); + + // Initialize the matrix with the appropriate size and capacity. + max_num_nonzeros_ = 0; + set_num_nonzeros(0); + Reserve(proto.num_nonzeros()); + Resize(proto.num_rows(), proto.num_cols()); + set_num_nonzeros(proto.num_nonzeros()); + + // Copy the entries in. + for (int i = 0; i < proto.num_nonzeros(); ++i) { + rows_[i] = proto.rows(i); + cols_[i] = proto.cols(i); + values_[i] = proto.values(i); + } +} +#endif + +TripletSparseMatrix& TripletSparseMatrix::operator=( + const TripletSparseMatrix& rhs) { + num_rows_ = rhs.num_rows_; + num_cols_ = rhs.num_cols_; + num_nonzeros_ = rhs.num_nonzeros_; + max_num_nonzeros_ = rhs.max_num_nonzeros_; + AllocateMemory(); + CopyData(rhs); + return *this; +} + +bool TripletSparseMatrix::AllTripletsWithinBounds() const { + for (int i = 0; i < num_nonzeros_; ++i) { + if ((rows_[i] < 0) || (rows_[i] >= num_rows_) || + (cols_[i] < 0) || (cols_[i] >= num_cols_)) + return false; + } + return true; +} + +void TripletSparseMatrix::Reserve(int new_max_num_nonzeros) { + CHECK_LE(num_nonzeros_, new_max_num_nonzeros) + << "Reallocation will cause data loss"; + + // Nothing to do if we have enough space already. + if (new_max_num_nonzeros <= max_num_nonzeros_) + return; + + int* new_rows = new int[new_max_num_nonzeros]; + int* new_cols = new int[new_max_num_nonzeros]; + double* new_values = new double[new_max_num_nonzeros]; + + for (int i = 0; i < num_nonzeros_; ++i) { + new_rows[i] = rows_[i]; + new_cols[i] = cols_[i]; + new_values[i] = values_[i]; + } + + rows_.reset(new_rows); + cols_.reset(new_cols); + values_.reset(new_values); + + max_num_nonzeros_ = new_max_num_nonzeros; +} + +void TripletSparseMatrix::SetZero() { + fill(values_.get(), values_.get() + max_num_nonzeros_, 0.0); + num_nonzeros_ = 0; +} + +void TripletSparseMatrix::set_num_nonzeros(int num_nonzeros) { + CHECK_GE(num_nonzeros, 0); + CHECK_LE(num_nonzeros, max_num_nonzeros_); + num_nonzeros_ = num_nonzeros; +}; + +void TripletSparseMatrix::AllocateMemory() { + rows_.reset(new int[max_num_nonzeros_]); + cols_.reset(new int[max_num_nonzeros_]); + values_.reset(new double[max_num_nonzeros_]); +} + +void TripletSparseMatrix::CopyData(const TripletSparseMatrix& orig) { + for (int i = 0; i < num_nonzeros_; ++i) { + rows_[i] = orig.rows_[i]; + cols_[i] = orig.cols_[i]; + values_[i] = orig.values_[i]; + } +} + +void TripletSparseMatrix::RightMultiply(const double* x, double* y) const { + for (int i = 0; i < num_nonzeros_; ++i) { + y[rows_[i]] += values_[i]*x[cols_[i]]; + } +} + +void TripletSparseMatrix::LeftMultiply(const double* x, double* y) const { + for (int i = 0; i < num_nonzeros_; ++i) { + y[cols_[i]] += values_[i]*x[rows_[i]]; + } +} + +void TripletSparseMatrix::SquaredColumnNorm(double* x) const { + CHECK_NOTNULL(x); + VectorRef(x, num_cols_).setZero(); + for (int i = 0; i < num_nonzeros_; ++i) { + x[cols_[i]] += values_[i] * values_[i]; + } +} + +void TripletSparseMatrix::ScaleColumns(const double* scale) { + CHECK_NOTNULL(scale); + for (int i = 0; i < num_nonzeros_; ++i) { + values_[i] = values_[i] * scale[cols_[i]]; + } +} + +void TripletSparseMatrix::ToDenseMatrix(Matrix* dense_matrix) const { + dense_matrix->resize(num_rows_, num_cols_); + dense_matrix->setZero(); + Matrix& m = *dense_matrix; + for (int i = 0; i < num_nonzeros_; ++i) { + m(rows_[i], cols_[i]) += values_[i]; + } +} + +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS +void TripletSparseMatrix::ToProto(SparseMatrixProto *proto) const { + proto->Clear(); + + TripletSparseMatrixProto* tsm_proto = proto->mutable_triplet_matrix(); + tsm_proto->set_num_rows(num_rows_); + tsm_proto->set_num_cols(num_cols_); + tsm_proto->set_num_nonzeros(num_nonzeros_); + for (int i = 0; i < num_nonzeros_; ++i) { + tsm_proto->add_rows(rows_[i]); + tsm_proto->add_cols(cols_[i]); + tsm_proto->add_values(values_[i]); + } +} +#endif + +void TripletSparseMatrix::AppendRows(const TripletSparseMatrix& B) { + CHECK_EQ(B.num_cols(), num_cols_); + Reserve(num_nonzeros_ + B.num_nonzeros_); + for (int i = 0; i < B.num_nonzeros_; ++i) { + rows_.get()[num_nonzeros_] = B.rows()[i] + num_rows_; + cols_.get()[num_nonzeros_] = B.cols()[i]; + values_.get()[num_nonzeros_++] = B.values()[i]; + } + num_rows_ = num_rows_ + B.num_rows(); +} + +void TripletSparseMatrix::AppendCols(const TripletSparseMatrix& B) { + CHECK_EQ(B.num_rows(), num_rows_); + Reserve(num_nonzeros_ + B.num_nonzeros_); + for (int i = 0; i < B.num_nonzeros_; ++i, ++num_nonzeros_) { + rows_.get()[num_nonzeros_] = B.rows()[i]; + cols_.get()[num_nonzeros_] = B.cols()[i] + num_cols_; + values_.get()[num_nonzeros_] = B.values()[i]; + } + num_cols_ = num_cols_ + B.num_cols(); +} + + +void TripletSparseMatrix::Resize(int new_num_rows, int new_num_cols) { + if ((new_num_rows >= num_rows_) && (new_num_cols >= num_cols_)) { + num_rows_ = new_num_rows; + num_cols_ = new_num_cols; + return; + } + + num_rows_ = new_num_rows; + num_cols_ = new_num_cols; + + int* r_ptr = rows_.get(); + int* c_ptr = cols_.get(); + double* v_ptr = values_.get(); + + int dropped_terms = 0; + for (int i = 0; i < num_nonzeros_; ++i) { + if ((r_ptr[i] < num_rows_) && (c_ptr[i] < num_cols_)) { + if (dropped_terms) { + r_ptr[i-dropped_terms] = r_ptr[i]; + c_ptr[i-dropped_terms] = c_ptr[i]; + v_ptr[i-dropped_terms] = v_ptr[i]; + } + } else { + ++dropped_terms; + } + } + num_nonzeros_ -= dropped_terms; +} + +TripletSparseMatrix* TripletSparseMatrix::CreateSparseDiagonalMatrix( + const double* values, int num_rows) { + TripletSparseMatrix* m = + new TripletSparseMatrix(num_rows, num_rows, num_rows); + for (int i = 0; i < num_rows; ++i) { + m->mutable_rows()[i] = i; + m->mutable_cols()[i] = i; + m->mutable_values()[i] = values[i]; + } + m->set_num_nonzeros(num_rows); + return m; +} + +void TripletSparseMatrix::ToTextFile(FILE* file) const { + CHECK_NOTNULL(file); + for (int i = 0; i < num_nonzeros_; ++i) { + fprintf(file, "% 10d % 10d %17f\n", rows_[i], cols_[i], values_[i]); + } +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/triplet_sparse_matrix.h b/extern/libmv/third_party/ceres/internal/ceres/triplet_sparse_matrix.h new file mode 100644 index 00000000000..300e74d0bbc --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/triplet_sparse_matrix.h @@ -0,0 +1,137 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#ifndef CERES_INTERNAL_TRIPLET_SPARSE_MATRIX_H_ +#define CERES_INTERNAL_TRIPLET_SPARSE_MATRIX_H_ + +#include "ceres/sparse_matrix.h" +#include "ceres/internal/eigen.h" +#include "ceres/internal/scoped_ptr.h" +#include "ceres/types.h" + +namespace ceres { +namespace internal { + +class SparseMatrixProto; + +// An implementation of the SparseMatrix interface to store and +// manipulate sparse matrices in triplet (i,j,s) form. This object is +// inspired by the design of the cholmod_triplet struct used in the +// SuiteSparse package and is memory layout compatible with it. +class TripletSparseMatrix : public SparseMatrix { + public: + TripletSparseMatrix(); + TripletSparseMatrix(int num_rows, int num_cols, int max_num_nonzeros); + explicit TripletSparseMatrix(const TripletSparseMatrix& orig); +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS + explicit TripletSparseMatrix(const SparseMatrixProto& proto); +#endif + + TripletSparseMatrix& operator=(const TripletSparseMatrix& rhs); + + ~TripletSparseMatrix(); + + // Implementation of the SparseMatrix interface. + virtual void SetZero(); + virtual void RightMultiply(const double* x, double* y) const; + virtual void LeftMultiply(const double* x, double* y) const; + virtual void SquaredColumnNorm(double* x) const; + virtual void ScaleColumns(const double* scale); + virtual void ToDenseMatrix(Matrix* dense_matrix) const; +#ifndef CERES_DONT_HAVE_PROTOCOL_BUFFERS + virtual void ToProto(SparseMatrixProto *proto) const; +#endif + virtual void ToTextFile(FILE* file) const; + virtual int num_rows() const { return num_rows_; } + virtual int num_cols() const { return num_cols_; } + virtual int num_nonzeros() const { return num_nonzeros_; } + virtual const double* values() const { return values_.get(); } + virtual double* mutable_values() { return values_.get(); } + virtual void set_num_nonzeros(int num_nonzeros); + + // Increase max_num_nonzeros and correspondingly increase the size + // of rows_, cols_ and values_. If new_max_num_nonzeros is smaller + // than max_num_nonzeros_, then num_non_zeros should be less than or + // equal to new_max_num_nonzeros, otherwise data loss is possible + // and the method crashes. + void Reserve(int new_max_num_nonzeros); + + // Append the matrix B at the bottom of this matrix. B should have + // the same number of columns as num_cols_. + void AppendRows(const TripletSparseMatrix& B); + + // Append the matrix B at the right of this matrix. B should have + // the same number of rows as num_rows_; + void AppendCols(const TripletSparseMatrix& B); + + // Resize the matrix. Entries which fall outside the new matrix + // bounds are dropped and the num_non_zeros changed accordingly. + void Resize(int new_num_rows, int new_num_cols); + + int max_num_nonzeros() const { return max_num_nonzeros_; } + const int* rows() const { return rows_.get(); } + const int* cols() const { return cols_.get(); } + int* mutable_rows() { return rows_.get(); } + int* mutable_cols() { return cols_.get(); } + + // Returns true if the entries of the matrix obey the row, column, + // and column size bounds and false otherwise. + bool AllTripletsWithinBounds() const; + + bool IsValid() const { return AllTripletsWithinBounds(); } + + // Build a sparse diagonal matrix of size num_rows x num_rows from + // the array values. Entries of the values array are copied into the + // sparse matrix. + static TripletSparseMatrix* CreateSparseDiagonalMatrix(const double* values, + int num_rows); + + private: + void AllocateMemory(); + void CopyData(const TripletSparseMatrix& orig); + + int num_rows_; + int num_cols_; + int max_num_nonzeros_; + int num_nonzeros_; + + // The data is stored as three arrays. For each i, values_[i] is + // stored at the location (rows_[i], cols_[i]). If the there are + // multiple entries with the same (rows_[i], cols_[i]), the values_ + // entries corresponding to them are summed up. + scoped_array<int> rows_; + scoped_array<int> cols_; + scoped_array<double> values_; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_TRIPLET_SPARSE_MATRIX_H__ diff --git a/extern/libmv/third_party/ceres/internal/ceres/types.cc b/extern/libmv/third_party/ceres/internal/ceres/types.cc new file mode 100644 index 00000000000..860f8a43f37 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/types.cc @@ -0,0 +1,98 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include <string> +#include "ceres/types.h" + +namespace ceres { + +#define CASESTR(x) case x: return #x + +const char* LinearSolverTypeToString(LinearSolverType solver_type) { + switch (solver_type) { + CASESTR(SPARSE_NORMAL_CHOLESKY); + CASESTR(DENSE_QR); + CASESTR(DENSE_SCHUR); + CASESTR(SPARSE_SCHUR); + CASESTR(ITERATIVE_SCHUR); + CASESTR(CGNR); + default: + return "UNKNOWN"; + } +} + +const char* PreconditionerTypeToString( + PreconditionerType preconditioner_type) { + switch (preconditioner_type) { + CASESTR(IDENTITY); + CASESTR(JACOBI); + CASESTR(SCHUR_JACOBI); + CASESTR(CLUSTER_JACOBI); + CASESTR(CLUSTER_TRIDIAGONAL); + default: + return "UNKNOWN"; + } +} + +const char* OrderingTypeToString(OrderingType ordering_type) { + switch (ordering_type) { + CASESTR(NATURAL); + CASESTR(USER); + CASESTR(SCHUR); + default: + return "UNKNOWN"; + } +} + +const char* SolverTerminationTypeToString( + SolverTerminationType termination_type) { + switch (termination_type) { + CASESTR(NO_CONVERGENCE); + CASESTR(FUNCTION_TOLERANCE); + CASESTR(GRADIENT_TOLERANCE); + CASESTR(PARAMETER_TOLERANCE); + CASESTR(NUMERICAL_FAILURE); + CASESTR(USER_ABORT); + CASESTR(USER_SUCCESS); + CASESTR(DID_NOT_RUN); + default: + return "UNKNOWN"; + } +} + +#undef CASESTR + +bool IsSchurType(LinearSolverType type) { + return ((type == SPARSE_SCHUR) || + (type == DENSE_SCHUR) || + (type == ITERATIVE_SCHUR)); +} + +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/visibility.cc b/extern/libmv/third_party/ceres/internal/ceres/visibility.cc new file mode 100644 index 00000000000..fd41648a7af --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/visibility.cc @@ -0,0 +1,150 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: kushalav@google.com (Avanish Kushal) + +#include <cmath> +#include <ctime> +#include <algorithm> +#include <set> +#include <vector> +#include <utility> + +#include <glog/logging.h> +#include "ceres/block_structure.h" +#include "ceres/collections_port.h" +#include "ceres/graph.h" + +namespace ceres { +namespace internal { + +void ComputeVisibility(const CompressedRowBlockStructure& block_structure, + const int num_eliminate_blocks, + vector< set<int> >* visibility) { + CHECK_NOTNULL(visibility); + + // Clear the visibility vector and resize it to hold a + // vector for each camera. + visibility->resize(0); + visibility->resize(block_structure.cols.size() - num_eliminate_blocks); + + for (int i = 0; i < block_structure.rows.size(); ++i) { + const vector<Cell>& cells = block_structure.rows[i].cells; + int block_id = cells[0].block_id; + // If the first block is not an e_block, then skip this row block. + if (block_id >= num_eliminate_blocks) { + continue; + } + + for (int j = 1; j < cells.size(); ++j) { + int camera_block_id = cells[j].block_id - num_eliminate_blocks; + DCHECK_GE(camera_block_id, 0); + DCHECK_LT(camera_block_id, visibility->size()); + (*visibility)[camera_block_id].insert(block_id); + } + } +} + +Graph<int>* CreateSchurComplementGraph(const vector<set<int> >& visibility) { + const time_t start_time = time(NULL); + // Compute the number of e_blocks/point blocks. Since the visibility + // set for each e_block/camera contains the set of e_blocks/points + // visible to it, we find the maximum across all visibility sets. + int num_points = 0; + for (int i = 0; i < visibility.size(); i++) { + if (visibility[i].size() > 0) { + num_points = max(num_points, (*visibility[i].rbegin()) + 1); + } + } + + // Invert the visibility. The input is a camera->point mapping, + // which tells us which points are visible in which + // cameras. However, to compute the sparsity structure of the Schur + // Complement efficiently, its better to have the point->camera + // mapping. + vector<set<int> > inverse_visibility(num_points); + for (int i = 0; i < visibility.size(); i++) { + const set<int>& visibility_set = visibility[i]; + for (set<int>::const_iterator it = visibility_set.begin(); + it != visibility_set.end(); + ++it) { + inverse_visibility[*it].insert(i); + } + } + + // Map from camera pairs to number of points visible to both cameras + // in the pair. + HashMap<pair<int, int>, int > camera_pairs; + + // Count the number of points visible to each camera/f_block pair. + for (vector<set<int> >::const_iterator it = inverse_visibility.begin(); + it != inverse_visibility.end(); + ++it) { + const set<int>& inverse_visibility_set = *it; + for (set<int>::const_iterator camera1 = inverse_visibility_set.begin(); + camera1 != inverse_visibility_set.end(); + ++camera1) { + set<int>::const_iterator camera2 = camera1; + for (++camera2; camera2 != inverse_visibility_set.end(); ++camera2) { + ++(camera_pairs[make_pair(*camera1, *camera2)]); + } + } + } + + Graph<int>* graph = new Graph<int>(); + + // Add vertices and initialize the pairs for self edges so that self + // edges are guaranteed. This is needed for the Canonical views + // algorithm to work correctly. + static const double kSelfEdgeWeight = 1.0; + for (int i = 0; i < visibility.size(); ++i) { + graph->AddVertex(i); + graph->AddEdge(i, i, kSelfEdgeWeight); + } + + // Add an edge for each camera pair. + for (HashMap<pair<int, int>, int>::const_iterator it = camera_pairs.begin(); + it != camera_pairs.end(); + ++it) { + const int camera1 = it->first.first; + const int camera2 = it->first.second; + CHECK_NE(camera1, camera2); + + const int count = it->second; + // Static cast necessary for Windows. + const double weight = static_cast<double>(count) / + (sqrt(static_cast<double>(visibility[camera1].size() * visibility[camera2].size()))); + graph->AddEdge(camera1, camera2, weight); + } + + VLOG(2) << "Schur complement graph time: " << (time(NULL) - start_time); + return graph; +} + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/visibility.h b/extern/libmv/third_party/ceres/internal/ceres/visibility.h new file mode 100644 index 00000000000..692dd87201e --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/visibility.h @@ -0,0 +1,77 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: kushalav@google.com (Avanish Kushal) +// sameeragarwal@google.com (Sameer Agarwal) +// +// Functions to manipulate visibility information from the block +// structure of sparse matrices. + +#ifndef CERES_INTERNAL_VISIBILITY_H_ +#define CERES_INTERNAL_VISIBILITY_H_ + +#include <set> +#include <vector> +#include "ceres/graph.h" + +namespace ceres { +namespace internal { + +class CompressedRowBlockStructure; + +// Given a compressed row block structure, computes the set of +// e_blocks "visible" to each f_block. If an e_block co-occurs with an +// f_block in a residual block, it is visible to the f_block. The +// first num_eliminate_blocks columns blocks are e_blocks and the rest +// f_blocks. +// +// In a structure from motion problem, e_blocks correspond to 3D +// points and f_blocks correspond to cameras. +void ComputeVisibility(const CompressedRowBlockStructure& block_structure, + int num_eliminate_blocks, + vector<set<int> >* visibility); + +// Given f_block visibility as computed by the ComputeVisibility +// function above, construct and return a graph whose vertices are +// f_blocks and an edge connects two vertices if they have atleast one +// e_block in common. The weight of this edge is normalized dot +// product between the visibility vectors of the two +// vertices/f_blocks. +// +// This graph reflects the sparsity structure of reduced camera +// matrix/Schur complement matrix obtained by eliminating the e_blocks +// from the normal equations. +// +// Caller acquires ownership of the returned Graph pointer +// (heap-allocated). +Graph<int>* CreateSchurComplementGraph(const vector<set<int> >& visibility); + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_VISIBILITY_H_ diff --git a/extern/libmv/third_party/ceres/internal/ceres/visibility_based_preconditioner.cc b/extern/libmv/third_party/ceres/internal/ceres/visibility_based_preconditioner.cc new file mode 100644 index 00000000000..aca77528215 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/visibility_based_preconditioner.cc @@ -0,0 +1,611 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) + +#include "ceres/visibility_based_preconditioner.h" + +#include <algorithm> +#include <functional> +#include <iterator> +#include <numeric> +#include <set> +#include <utility> +#include <vector> +#include <glog/logging.h> +#include "Eigen/Dense" +#include "ceres/block_random_access_sparse_matrix.h" +#include "ceres/block_sparse_matrix.h" +#include "ceres/canonical_views_clustering.h" +#include "ceres/collections_port.h" +#include "ceres/detect_structure.h" +#include "ceres/graph.h" +#include "ceres/graph_algorithms.h" +#include "ceres/linear_solver.h" +#include "ceres/schur_eliminator.h" +#include "ceres/visibility.h" +#include "ceres/internal/scoped_ptr.h" + +namespace ceres { +namespace internal { + +// TODO(sameeragarwal): Currently these are magic weights for the +// preconditioner construction. Move these higher up into the Options +// struct and provide some guidelines for choosing them. +// +// This will require some more work on the clustering algorithm and +// possibly some more refactoring of the code. +static const double kSizePenaltyWeight = 3.0; +static const double kSimilarityPenaltyWeight = 0.0; + +#ifndef CERES_NO_SUITESPARSE +VisibilityBasedPreconditioner::VisibilityBasedPreconditioner( + const CompressedRowBlockStructure& bs, + const LinearSolver::Options& options) + : options_(options), + num_blocks_(0), + num_clusters_(0), + factor_(NULL) { + CHECK_GT(options_.num_eliminate_blocks, 0); + CHECK(options_.preconditioner_type == SCHUR_JACOBI || + options_.preconditioner_type == CLUSTER_JACOBI || + options_.preconditioner_type == CLUSTER_TRIDIAGONAL) + << "Unknown preconditioner type: " << options_.preconditioner_type; + num_blocks_ = bs.cols.size() - options_.num_eliminate_blocks; + CHECK_GT(num_blocks_, 0) + << "Jacobian should have atleast 1 f_block for " + << "visibility based preconditioning."; + + // Vector of camera block sizes + block_size_.resize(num_blocks_); + for (int i = 0; i < num_blocks_; ++i) { + block_size_[i] = bs.cols[i + options_.num_eliminate_blocks].size; + } + + const time_t start_time = time(NULL); + switch (options_.preconditioner_type) { + case SCHUR_JACOBI: + ComputeSchurJacobiSparsity(bs); + break; + case CLUSTER_JACOBI: + ComputeClusterJacobiSparsity(bs); + break; + case CLUSTER_TRIDIAGONAL: + ComputeClusterTridiagonalSparsity(bs); + break; + default: + LOG(FATAL) << "Unknown preconditioner type"; + } + const time_t structure_time = time(NULL); + InitStorage(bs); + const time_t storage_time = time(NULL); + InitEliminator(bs); + const time_t eliminator_time = time(NULL); + + // Allocate temporary storage for a vector used during + // RightMultiply. + tmp_rhs_ = CHECK_NOTNULL(ss_.CreateDenseVector(NULL, + m_->num_rows(), + m_->num_rows())); + const time_t init_time = time(NULL); + VLOG(2) << "init time: " + << init_time - start_time + << " structure time: " << structure_time - start_time + << " storage time:" << storage_time - structure_time + << " eliminator time: " << eliminator_time - storage_time; +} + +VisibilityBasedPreconditioner::~VisibilityBasedPreconditioner() { + if (factor_ != NULL) { + ss_.Free(factor_); + factor_ = NULL; + } + if (tmp_rhs_ != NULL) { + ss_.Free(tmp_rhs_); + tmp_rhs_ = NULL; + } +} + +// Determine the sparsity structure of the SCHUR_JACOBI +// preconditioner. SCHUR_JACOBI is an extreme case of a visibility +// based preconditioner where each camera block corresponds to a +// cluster and there is no interaction between clusters. +void VisibilityBasedPreconditioner::ComputeSchurJacobiSparsity( + const CompressedRowBlockStructure& bs) { + num_clusters_ = num_blocks_; + cluster_membership_.resize(num_blocks_); + cluster_pairs_.clear(); + + // Each camea block is a member of its own cluster and the only + // cluster pairs are the self edges (i,i). + for (int i = 0; i < num_clusters_; ++i) { + cluster_membership_[i] = i; + cluster_pairs_.insert(make_pair(i, i)); + } +} + +// Determine the sparsity structure of the CLUSTER_JACOBI +// preconditioner. It clusters cameras using their scene +// visibility. The clusters form the diagonal blocks of the +// preconditioner matrix. +void VisibilityBasedPreconditioner::ComputeClusterJacobiSparsity( + const CompressedRowBlockStructure& bs) { + vector<set<int> > visibility; + ComputeVisibility(bs, options_.num_eliminate_blocks, &visibility); + CHECK_EQ(num_blocks_, visibility.size()); + ClusterCameras(visibility); + cluster_pairs_.clear(); + for (int i = 0; i < num_clusters_; ++i) { + cluster_pairs_.insert(make_pair(i, i)); + } +} + +// Determine the sparsity structure of the CLUSTER_TRIDIAGONAL +// preconditioner. It clusters cameras using using the scene +// visibility and then finds the strongly interacting pairs of +// clusters by constructing another graph with the clusters as +// vertices and approximating it with a degree-2 maximum spanning +// forest. The set of edges in this forest are the cluster pairs. +void VisibilityBasedPreconditioner::ComputeClusterTridiagonalSparsity( + const CompressedRowBlockStructure& bs) { + vector<set<int> > visibility; + ComputeVisibility(bs, options_.num_eliminate_blocks, &visibility); + CHECK_EQ(num_blocks_, visibility.size()); + ClusterCameras(visibility); + + // Construct a weighted graph on the set of clusters, where the + // edges are the number of 3D points/e_blocks visible in both the + // clusters at the ends of the edge. Return an approximate degree-2 + // maximum spanning forest of this graph. + vector<set<int> > cluster_visibility; + ComputeClusterVisibility(visibility, &cluster_visibility); + scoped_ptr<Graph<int> > cluster_graph( + CHECK_NOTNULL(CreateClusterGraph(cluster_visibility))); + scoped_ptr<Graph<int> > forest( + CHECK_NOTNULL(Degree2MaximumSpanningForest(*cluster_graph))); + ForestToClusterPairs(*forest, &cluster_pairs_); +} + +// Allocate storage for the preconditioner matrix. +void VisibilityBasedPreconditioner::InitStorage( + const CompressedRowBlockStructure& bs) { + ComputeBlockPairsInPreconditioner(bs); + m_.reset(new BlockRandomAccessSparseMatrix(block_size_, block_pairs_)); +} + +// Call the canonical views algorithm and cluster the cameras based on +// their visibility sets. The visibility set of a camera is the set of +// e_blocks/3D points in the scene that are seen by it. +// +// The cluster_membership_ vector is updated to indicate cluster +// memberships for each camera block. +void VisibilityBasedPreconditioner::ClusterCameras( + const vector<set<int> >& visibility) { + scoped_ptr<Graph<int> > schur_complement_graph( + CHECK_NOTNULL(CreateSchurComplementGraph(visibility))); + + CanonicalViewsClusteringOptions options; + options.size_penalty_weight = kSizePenaltyWeight; + options.similarity_penalty_weight = kSimilarityPenaltyWeight; + + vector<int> centers; + HashMap<int, int> membership; + ComputeCanonicalViewsClustering(*schur_complement_graph, + options, + ¢ers, + &membership); + num_clusters_ = centers.size(); + CHECK_GT(num_clusters_, 0); + VLOG(2) << "num_clusters: " << num_clusters_; + FlattenMembershipMap(membership, &cluster_membership_); +} + +// Compute the block sparsity structure of the Schur complement +// matrix. For each pair of cameras contributing a non-zero cell to +// the schur complement, determine if that cell is present in the +// preconditioner or not. +// +// A pair of cameras contribute a cell to the preconditioner if they +// are part of the same cluster or if the the two clusters that they +// belong have an edge connecting them in the degree-2 maximum +// spanning forest. +// +// For example, a camera pair (i,j) where i belonges to cluster1 and +// j belongs to cluster2 (assume that cluster1 < cluster2). +// +// The cell corresponding to (i,j) is present in the preconditioner +// if cluster1 == cluster2 or the pair (cluster1, cluster2) were +// connected by an edge in the degree-2 maximum spanning forest. +// +// Since we have already expanded the forest into a set of camera +// pairs/edges, including self edges, the check can be reduced to +// checking membership of (cluster1, cluster2) in cluster_pairs_. +void VisibilityBasedPreconditioner::ComputeBlockPairsInPreconditioner( + const CompressedRowBlockStructure& bs) { + block_pairs_.clear(); + for (int i = 0; i < num_blocks_; ++i) { + block_pairs_.insert(make_pair(i, i)); + } + + int r = 0; + set<pair<int, int> > skipped_pairs; + const int num_row_blocks = bs.rows.size(); + const int num_eliminate_blocks = options_.num_eliminate_blocks; + + // Iterate over each row of the matrix. The block structure of the + // matrix is assumed to be sorted in order of the e_blocks/point + // blocks. Thus all row blocks containing an e_block/point occur + // contiguously. Further, if present, an e_block is always the first + // parameter block in each row block. These structural assumptions + // are common to all Schur complement based solvers in Ceres. + // + // For each e_block/point block we identify the set of cameras + // seeing it. The cross product of this set with itself is the set + // of non-zero cells contibuted by this e_block. + // + // The time complexity of this is O(nm^2) where, n is the number of + // 3d points and m is the maximum number of cameras seeing any + // point, which for most scenes is a fairly small number. + while (r < num_row_blocks) { + int e_block_id = bs.rows[r].cells.front().block_id; + if (e_block_id >= num_eliminate_blocks) { + // Skip the rows whose first block is an f_block. + break; + } + + set<int> f_blocks; + for (; r < num_row_blocks; ++r) { + const CompressedRow& row = bs.rows[r]; + if (row.cells.front().block_id != e_block_id) { + break; + } + + // Iterate over the blocks in the row, ignoring the first block + // since it is the one to be eliminated and adding the rest to + // the list of f_blocks associated with this e_block. + for (int c = 1; c < row.cells.size(); ++c) { + const Cell& cell = row.cells[c]; + const int f_block_id = cell.block_id - num_eliminate_blocks; + CHECK_GE(f_block_id, 0); + f_blocks.insert(f_block_id); + } + } + + for (set<int>::const_iterator block1 = f_blocks.begin(); + block1 != f_blocks.end(); + ++block1) { + set<int>::const_iterator block2 = block1; + ++block2; + for (; block2 != f_blocks.end(); ++block2) { + if (IsBlockPairInPreconditioner(*block1, *block2)) { + block_pairs_.insert(make_pair(*block1, *block2)); + } else { + skipped_pairs.insert(make_pair(*block1, *block2)); + } + } + } + } + + // The remaining rows which do not contain any e_blocks. + for (; r < num_row_blocks; ++r) { + const CompressedRow& row = bs.rows[r]; + CHECK_GE(row.cells.front().block_id, num_eliminate_blocks); + for (int i = 0; i < row.cells.size(); ++i) { + const int block1 = row.cells[i].block_id - num_eliminate_blocks; + for (int j = 0; j < row.cells.size(); ++j) { + const int block2 = row.cells[j].block_id - num_eliminate_blocks; + if (block1 <= block2) { + if (IsBlockPairInPreconditioner(block1, block2)) { + block_pairs_.insert(make_pair(block1, block2)); + } else { + skipped_pairs.insert(make_pair(block1, block2)); + } + } + } + } + } + + VLOG(1) << "Block pair stats: " + << block_pairs_.size() << " included " + << skipped_pairs.size() << " excluded"; +} + +// Initialize the SchurEliminator. +void VisibilityBasedPreconditioner::InitEliminator( + const CompressedRowBlockStructure& bs) { + LinearSolver::Options eliminator_options; + eliminator_options.num_eliminate_blocks = options_.num_eliminate_blocks; + eliminator_options.num_threads = options_.num_threads; + eliminator_options.constant_sparsity = true; + + DetectStructure(bs, options_.num_eliminate_blocks, + &eliminator_options.row_block_size, + &eliminator_options.e_block_size, + &eliminator_options.f_block_size); + + eliminator_.reset(SchurEliminatorBase::Create(eliminator_options)); + eliminator_->Init(options_.num_eliminate_blocks, &bs); +} + +// Compute the values of the preconditioner matrix and factorize it. +bool VisibilityBasedPreconditioner::Compute(const BlockSparseMatrixBase& A, + const double* D) { + const time_t start_time = time(NULL); + const int num_rows = m_->num_rows(); + CHECK_GT(num_rows, 0); + + // We need a dummy rhs vector and a dummy b vector since the Schur + // eliminator combines the computation of the reduced camera matrix + // with the computation of the right hand side of that linear + // system. + // + // TODO(sameeragarwal): Perhaps its worth refactoring the + // SchurEliminator::Eliminate function to allow NULL for the rhs. As + // of now it does not seem to be worth the effort. + Vector rhs = Vector::Zero(m_->num_rows()); + Vector b = Vector::Zero(A.num_rows()); + + // Compute a subset of the entries of the Schur complement. + eliminator_->Eliminate(&A, b.data(), D, m_.get(), rhs.data()); + + // Try factorizing the matrix. For SCHUR_JACOBI and CLUSTER_JACOBI, + // this should always succeed modulo some numerical/conditioning + // problems. For CLUSTER_TRIDIAGONAL, in general the preconditioner + // matrix as constructed is not positive definite. However, we will + // go ahead and try factorizing it. If it works, great, otherwise we + // scale all the cells in the preconditioner corresponding to the + // edges in the degree-2 forest and that guarantees positive + // definiteness. The proof of this fact can be found in Lemma 1 in + // "Visibility Based Preconditioning for Bundle Adjustment". + // + // Doing the factorization like this saves us matrix mass when + // scaling is not needed, which is quite often in our experience. + bool status = Factorize(); + + // The scaling only affects the tri-diagonal case, since + // ScaleOffDiagonalBlocks only pays attenion to the cells that + // belong to the edges of the degree-2 forest. In the SCHUR_JACOBI + // and the CLUSTER_JACOBI cases, the preconditioner is guaranteed to + // be positive semidefinite. + if (!status && options_.preconditioner_type == CLUSTER_TRIDIAGONAL) { + VLOG(1) << "Unscaled factorization failed. Retrying with off-diagonal " + << "scaling"; + ScaleOffDiagonalCells(); + status = Factorize(); + } + + VLOG(2) << "Compute time: " << time(NULL) - start_time; + return status; +} + +// Consider the preconditioner matrix as meta-block matrix, whose +// blocks correspond to the clusters. Then cluster pairs corresponding +// to edges in the degree-2 forest are off diagonal entries of this +// matrix. Scaling these off-diagonal entries by 1/2 forces this +// matrix to be positive definite. +void VisibilityBasedPreconditioner::ScaleOffDiagonalCells() { + for (set< pair<int, int> >::const_iterator it = block_pairs_.begin(); + it != block_pairs_.end(); + ++it) { + const int block1 = it->first; + const int block2 = it->second; + if (!IsBlockPairOffDiagonal(block1, block2)) { + continue; + } + + int r, c, row_stride, col_stride; + CellInfo* cell_info = m_->GetCell(block1, block2, + &r, &c, + &row_stride, &col_stride); + CHECK(cell_info != NULL) + << "Cell missing for block pair (" << block1 << "," << block2 << ")" + << " cluster pair (" << cluster_membership_[block1] + << " " << cluster_membership_[block2] << ")"; + + // Ah the magic of tri-diagonal matrices and diagonal + // dominance. See Lemma 1 in "Visibility Based Preconditioning + // For Bundle Adjustment". + MatrixRef m(cell_info->values, row_stride, col_stride); + m.block(r, c, block_size_[block1], block_size_[block2]) *= 0.5; + } +} + +// Compute the sparse Cholesky factorization of the preconditioner +// matrix. +bool VisibilityBasedPreconditioner::Factorize() { + // Extract the TripletSparseMatrix that is used for actually storing + // S and convert it into a cholmod_sparse object. + cholmod_sparse* lhs = ss_.CreateSparseMatrix( + down_cast<BlockRandomAccessSparseMatrix*>( + m_.get())->mutable_matrix()); + + // The matrix is symmetric, and the upper triangular part of the + // matrix contains the values. + lhs->stype = 1; + + // Symbolic factorization is computed if we don't already have one + // handy. + if (factor_ == NULL) { + factor_ = ss_.AnalyzeCholesky(lhs); + } + + bool status = ss_.Cholesky(lhs, factor_); + ss_.Free(lhs); + return status; +} + +void VisibilityBasedPreconditioner::RightMultiply(const double* x, + double* y) const { + CHECK_NOTNULL(x); + CHECK_NOTNULL(y); + SuiteSparse* ss = const_cast<SuiteSparse*>(&ss_); + + const int num_rows = m_->num_rows(); + memcpy(CHECK_NOTNULL(tmp_rhs_)->x, x, m_->num_rows() * sizeof(*x)); + cholmod_dense* solution = CHECK_NOTNULL(ss->Solve(factor_, tmp_rhs_)); + memcpy(y, solution->x, sizeof(*y) * num_rows); + ss->Free(solution); +} + +int VisibilityBasedPreconditioner::num_rows() const { + return m_->num_rows(); +} + +// Classify camera/f_block pairs as in and out of the preconditioner, +// based on whether the cluster pair that they belong to is in the +// preconditioner or not. +bool VisibilityBasedPreconditioner::IsBlockPairInPreconditioner( + const int block1, + const int block2) const { + int cluster1 = cluster_membership_[block1]; + int cluster2 = cluster_membership_[block2]; + if (cluster1 > cluster2) { + std::swap(cluster1, cluster2); + } + return (cluster_pairs_.count(make_pair(cluster1, cluster2)) > 0); +} + +bool VisibilityBasedPreconditioner::IsBlockPairOffDiagonal( + const int block1, + const int block2) const { + return (cluster_membership_[block1] != cluster_membership_[block2]); +} + +// Convert a graph into a list of edges that includes self edges for +// each vertex. +void VisibilityBasedPreconditioner::ForestToClusterPairs( + const Graph<int>& forest, + HashSet<pair<int, int> >* cluster_pairs) const { + CHECK_NOTNULL(cluster_pairs)->clear(); + const HashSet<int>& vertices = forest.vertices(); + CHECK_EQ(vertices.size(), num_clusters_); + + // Add all the cluster pairs corresponding to the edges in the + // forest. + for (HashSet<int>::const_iterator it1 = vertices.begin(); + it1 != vertices.end(); + ++it1) { + const int cluster1 = *it1; + cluster_pairs->insert(make_pair(cluster1, cluster1)); + const HashSet<int>& neighbors = forest.Neighbors(cluster1); + for (HashSet<int>::const_iterator it2 = neighbors.begin(); + it2 != neighbors.end(); + ++it2) { + const int cluster2 = *it2; + if (cluster1 < cluster2) { + cluster_pairs->insert(make_pair(cluster1, cluster2)); + } + } + } +} + +// The visibilty set of a cluster is the union of the visibilty sets +// of all its cameras. In other words, the set of points visible to +// any camera in the cluster. +void VisibilityBasedPreconditioner::ComputeClusterVisibility( + const vector<set<int> >& visibility, + vector<set<int> >* cluster_visibility) const { + CHECK_NOTNULL(cluster_visibility)->resize(0); + cluster_visibility->resize(num_clusters_); + for (int i = 0; i < num_blocks_; ++i) { + const int cluster_id = cluster_membership_[i]; + (*cluster_visibility)[cluster_id].insert(visibility[i].begin(), + visibility[i].end()); + } +} + +// Construct a graph whose vertices are the clusters, and the edge +// weights are the number of 3D points visible to cameras in both the +// vertices. +Graph<int>* VisibilityBasedPreconditioner::CreateClusterGraph( + const vector<set<int> >& cluster_visibility) const { + Graph<int>* cluster_graph = new Graph<int>; + + for (int i = 0; i < num_clusters_; ++i) { + cluster_graph->AddVertex(i); + } + + for (int i = 0; i < num_clusters_; ++i) { + const set<int>& cluster_i = cluster_visibility[i]; + for (int j = i+1; j < num_clusters_; ++j) { + vector<int> intersection; + const set<int>& cluster_j = cluster_visibility[j]; + set_intersection(cluster_i.begin(), cluster_i.end(), + cluster_j.begin(), cluster_j.end(), + back_inserter(intersection)); + + if (intersection.size() > 0) { + // Clusters interact strongly when they share a large number + // of 3D points. The degree-2 maximum spanning forest + // alorithm, iterates on the edges in decreasing order of + // their weight, which is the number of points shared by the + // two cameras that it connects. + cluster_graph->AddEdge(i, j, intersection.size()); + } + } + } + return cluster_graph; +} + +// Canonical views clustering returns a HashMap from vertices to +// cluster ids. Convert this into a flat array for quick lookup. It is +// possible that some of the vertices may not be associated with any +// cluster. In that case, randomly assign them to one of the clusters. +void VisibilityBasedPreconditioner::FlattenMembershipMap( + const HashMap<int, int>& membership_map, + vector<int>* membership_vector) const { + CHECK_NOTNULL(membership_vector)->resize(0); + membership_vector->resize(num_blocks_, -1); + // Iterate over the cluster membership map and update the + // cluster_membership_ vector assigning arbitrary cluster ids to + // the few cameras that have not been clustered. + for (HashMap<int, int>::const_iterator it = membership_map.begin(); + it != membership_map.end(); + ++it) { + const int camera_id = it->first; + int cluster_id = it->second; + + // If the view was not clustered, randomly assign it to one of the + // clusters. This preserves the mathematical correctness of the + // preconditioner. If there are too many views which are not + // clustered, it may lead to some quality degradation though. + // + // TODO(sameeragarwal): Check if a large number of views have not + // been clustered and deal with it? + if (cluster_id == -1) { + cluster_id = camera_id % num_clusters_; + } + + membership_vector->at(camera_id) = cluster_id; + } +} + +#endif // CERES_NO_SUITESPARSE + +} // namespace internal +} // namespace ceres diff --git a/extern/libmv/third_party/ceres/internal/ceres/visibility_based_preconditioner.h b/extern/libmv/third_party/ceres/internal/ceres/visibility_based_preconditioner.h new file mode 100644 index 00000000000..fa095ca1dd8 --- /dev/null +++ b/extern/libmv/third_party/ceres/internal/ceres/visibility_based_preconditioner.h @@ -0,0 +1,273 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2010, 2011, 2012 Google Inc. All rights reserved. +// http://code.google.com/p/ceres-solver/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of Google Inc. nor the names of its contributors may be +// used to endorse or promote products derived from this software without +// specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Author: sameeragarwal@google.com (Sameer Agarwal) +// +// Preconditioners for linear systems that arise in Structure from +// Motion problems. VisibilityBasedPreconditioner implements three +// preconditioners: +// +// SCHUR_JACOBI +// CLUSTER_JACOBI +// CLUSTER_TRIDIAGONAL +// +// Detailed descriptions of these preconditions beyond what is +// documented here can be found in +// +// Bundle Adjustment in the Large +// S. Agarwal, N. Snavely, S. Seitz & R. Szeliski, ECCV 2010 +// http://www.cs.washington.edu/homes/sagarwal/bal.pdf +// +// Visibility Based Preconditioning for Bundle Adjustment +// A. Kushal & S. Agarwal, submitted to CVPR 2012 +// http://www.cs.washington.edu/homes/sagarwal/vbp.pdf +// +// The three preconditioners share enough code that its most efficient +// to implement them as part of the same code base. + +#ifndef CERES_INTERNAL_VISIBILITY_BASED_PRECONDITIONER_H_ +#define CERES_INTERNAL_VISIBILITY_BASED_PRECONDITIONER_H_ + +#include <set> +#include <vector> +#include <utility> +#include "ceres/collections_port.h" +#include "ceres/graph.h" +#include "ceres/linear_solver.h" +#include "ceres/linear_operator.h" +#include "ceres/suitesparse.h" +#include "ceres/internal/macros.h" +#include "ceres/internal/scoped_ptr.h" + +namespace ceres { +namespace internal { + +class BlockRandomAccessSparseMatrix; +class BlockSparseMatrixBase; +class CompressedRowBlockStructure; +class SchurEliminatorBase; + +// This class implements three preconditioners for Structure from +// Motion/Bundle Adjustment problems. The name +// VisibilityBasedPreconditioner comes from the fact that the sparsity +// structure of the preconditioner matrix is determined by analyzing +// the visibility structure of the scene, i.e. which cameras see which +// points. +// +// Strictly speaking, SCHUR_JACOBI is not a visibility based +// preconditioner but it is an extreme case of CLUSTER_JACOBI, where +// every cluster contains exactly one camera block. Treating it as a +// special case of CLUSTER_JACOBI makes it easy to implement as part +// of the same code base with no significant loss of performance. +// +// In the following, we will only discuss CLUSTER_JACOBI and +// CLUSTER_TRIDIAGONAL. +// +// The key idea of visibility based preconditioning is to identify +// cameras that we expect have strong interactions, and then using the +// entries in the Schur complement matrix corresponding to these +// camera pairs as an approximation to the full Schur complement. +// +// CLUSTER_JACOBI identifies these camera pairs by clustering cameras, +// and considering all non-zero camera pairs within each cluster. The +// clustering in the current implementation is done using the +// Canonical Views algorithm of Simon et al. (see +// canonical_views_clustering.h). For the purposes of clustering, the +// similarity or the degree of interaction between a pair of cameras +// is measured by counting the number of points visible in both the +// cameras. Thus the name VisibilityBasedPreconditioner. Further, if we +// were to permute the parameter blocks such that all the cameras in +// the same cluster occur contiguously, the preconditioner matrix will +// be a block diagonal matrix with blocks corresponding to the +// clusters. Thus in analogy with the Jacobi preconditioner we refer +// to this as the CLUSTER_JACOBI preconditioner. +// +// CLUSTER_TRIDIAGONAL adds more mass to the CLUSTER_JACOBI +// preconditioner by considering the interaction between clusters and +// identifying strong interactions between cluster pairs. This is done +// by constructing a weighted graph on the clusters, with the weight +// on the edges connecting two clusters proportional to the number of +// 3D points visible to cameras in both the clusters. A degree-2 +// maximum spanning forest is identified in this graph and the camera +// pairs contained in the edges of this forest are added to the +// preconditioner. The detailed reasoning for this construction is +// explained in the paper mentioned above. +// +// Degree-2 spanning trees and forests have the property that they +// correspond to tri-diagonal matrices. Thus there exist a permutation +// of the camera blocks under which the CLUSTER_TRIDIAGONAL +// preconditioner matrix is a block tridiagonal matrix, and thus the +// name for the preconditioner. +// +// Thread Safety: This class is NOT thread safe. +// +// Example usage: +// +// LinearSolver::Options options; +// options.preconditioner_type = CLUSTER_JACOBI; +// options.num_eliminate_blocks = num_points; +// VisibilityBasedPreconditioner preconditioner( +// *A.block_structure(), options); +// preconditioner.Compute(A, NULL); +// preconditioner.RightMultiply(x, y); +// + +#ifndef CERES_NO_SUITESPARSE +class VisibilityBasedPreconditioner : public LinearOperator { + public: + // Initialize the symbolic structure of the preconditioner. bs is + // the block structure of the linear system to be solved. It is used + // to determine the sparsity structure of the preconditioner matrix. + // + // It has the same structural requirement as other Schur complement + // based solvers. Please see schur_eliminator.h for more details. + // + // LinearSolver::Options::num_eliminate_blocks should be set to the + // number of e_blocks in the block structure. + // + // TODO(sameeragarwal): The use of LinearSolver::Options should + // ultimately be replaced with Preconditioner::Options and some sort + // of preconditioner factory along the lines of + // LinearSolver::CreateLinearSolver. I will wait to do this till I + // create a general purpose block Jacobi preconditioner for general + // sparse problems along with a CGLS solver. + VisibilityBasedPreconditioner(const CompressedRowBlockStructure& bs, + const LinearSolver::Options& options); + virtual ~VisibilityBasedPreconditioner(); + + // Compute the numerical value of the preconditioner for the linear + // system: + // + // | A | x = |b| + // |diag(D)| |0| + // + // for some vector b. It is important that the matrix A have the + // same block structure as the one used to construct this object. + // + // D can be NULL, in which case its interpreted as a diagonal matrix + // of size zero. + bool Compute(const BlockSparseMatrixBase& A, + const double* D); + + // LinearOperator interface. Since the operator is symmetric, + // LeftMultiply and num_cols are just calls to RightMultiply and + // num_rows respectively. Compute() must be called before + // RightMultiply can be called. + virtual void RightMultiply(const double* x, double* y) const; + virtual void LeftMultiply(const double* x, double* y) const { + RightMultiply(x, y); + } + virtual int num_rows() const; + virtual int num_cols() const { return num_rows(); } + + friend class VisibilityBasedPreconditionerTest; + private: + void ComputeSchurJacobiSparsity(const CompressedRowBlockStructure& bs); + void ComputeClusterJacobiSparsity(const CompressedRowBlockStructure& bs); + void ComputeClusterTridiagonalSparsity(const CompressedRowBlockStructure& bs); + void InitStorage(const CompressedRowBlockStructure& bs); + void InitEliminator(const CompressedRowBlockStructure& bs); + bool Factorize(); + void ScaleOffDiagonalCells(); + + void ClusterCameras(const vector< set<int> >& visibility); + void FlattenMembershipMap(const HashMap<int, int>& membership_map, + vector<int>* membership_vector) const; + void ComputeClusterVisibility(const vector<set<int> >& visibility, + vector<set<int> >* cluster_visibility) const; + Graph<int>* CreateClusterGraph(const vector<set<int> >& visibility) const; + void ForestToClusterPairs(const Graph<int>& forest, + HashSet<pair<int, int> >* cluster_pairs) const; + void ComputeBlockPairsInPreconditioner(const CompressedRowBlockStructure& bs); + bool IsBlockPairInPreconditioner(int block1, int block2) const; + bool IsBlockPairOffDiagonal(int block1, int block2) const; + + LinearSolver::Options options_; + + // Number of parameter blocks in the schur complement. + int num_blocks_; + int num_clusters_; + + // Sizes of the blocks in the schur complement. + vector<int> block_size_; + + // Mapping from cameras to clusters. + vector<int> cluster_membership_; + + // Non-zero camera pairs from the schur complement matrix that are + // present in the preconditioner, sorted by row (first element of + // each pair), then column (second). + set<pair<int, int> > block_pairs_; + + // Set of cluster pairs (including self pairs (i,i)) in the + // preconditioner. + HashSet<pair<int, int> > cluster_pairs_; + scoped_ptr<SchurEliminatorBase> eliminator_; + + // Preconditioner matrix. + scoped_ptr<BlockRandomAccessSparseMatrix> m_; + + // RightMultiply is a const method for LinearOperators. It is + // implemented using CHOLMOD's sparse triangular matrix solve + // function. This however requires non-const access to the + // SuiteSparse context object, even though it does not result in any + // of the state of the preconditioner being modified. + SuiteSparse ss_; + + // Symbolic and numeric factorization of the preconditioner. + cholmod_factor* factor_; + + // Temporary vector used by RightMultiply. + cholmod_dense* tmp_rhs_; + DISALLOW_COPY_AND_ASSIGN(VisibilityBasedPreconditioner); +}; +#else // SuiteSparse +// If SuiteSparse is not compiled in, the preconditioner is not +// available. +class VisibilityBasedPreconditioner : public LinearOperator { + public: + VisibilityBasedPreconditioner(const CompressedRowBlockStructure& bs, + const LinearSolver::Options& options) { + LOG(FATAL) << "Visibility based preconditioning is not available. Please " + "build Ceres with SuiteSparse."; + } + virtual ~VisibilityBasedPreconditioner() {} + virtual void RightMultiply(const double* x, double* y) const {} + virtual void LeftMultiply(const double* x, double* y) const {} + virtual int num_rows() const { return -1; } + virtual int num_cols() const { return -1; } + bool Compute(const BlockSparseMatrixBase& A, const double* D) { + return false; + } +}; +#endif // CERES_NO_SUITESPARSE + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_VISIBILITY_BASED_PRECONDITIONER_H_ diff --git a/extern/libmv/third_party/ceres/mkfiles.sh b/extern/libmv/third_party/ceres/mkfiles.sh new file mode 100644 index 00000000000..d335829aa2c --- /dev/null +++ b/extern/libmv/third_party/ceres/mkfiles.sh @@ -0,0 +1,4 @@ +#!/bin/sh + +find ./include/ -type f | sed -r 's/^\.\///' | sort > files.txt +find ./internal/ -type f | sed -r 's/^\.\///' | sort >> files.txt diff --git a/extern/libmv/third_party/ceres/patches/msvc_isfinite.patch b/extern/libmv/third_party/ceres/patches/msvc_isfinite.patch new file mode 100644 index 00000000000..c3129d8e02b --- /dev/null +++ b/extern/libmv/third_party/ceres/patches/msvc_isfinite.patch @@ -0,0 +1,15 @@ +diff --git a/internal/ceres/residual_block_utils.cc b/internal/ceres/residual_block_utils.cc +index ed3499b..28e0313 100644 +--- a/internal/ceres/residual_block_utils.cc ++++ b/internal/ceres/residual_block_utils.cc +@@ -40,6 +40,10 @@ + #include "ceres/internal/eigen.h" + #include "ceres/internal/port.h" + ++#ifdef _MSC_VER ++# define isfinite _finite ++#endif ++ + namespace ceres { + namespace internal { + diff --git a/extern/libmv/third_party/ceres/patches/series b/extern/libmv/third_party/ceres/patches/series new file mode 100644 index 00000000000..dbe955ae61e --- /dev/null +++ b/extern/libmv/third_party/ceres/patches/series @@ -0,0 +1 @@ +msvc_isfinite.patch |