Some formatting
							parent
							
								
									f712d62150
								
							
						
					
					
						commit
						0c622294d2
					
				|  | @ -147,68 +147,74 @@ public: | |||
|      * G = F' * F - F' * E * P * E' * F | ||||
|      * g = F' * (b - E * P * E' * b) | ||||
|      * Fixed size version | ||||
|      */ | ||||
|     template<int N, int ND> // N = 2 or 3 (point dimension), ND is the camera dimension
 | ||||
|     static SymmetricBlockMatrix SchurComplement( | ||||
|         const std::vector< Eigen::Matrix<double, ZDim, ND>, Eigen::aligned_allocator< Eigen::Matrix<double, ZDim, ND> > >& Fs, | ||||
|         const Matrix& E, const Eigen::Matrix<double, N, N>& P, const Vector& b) { | ||||
|     */ | ||||
|   template <int N, | ||||
|             int ND>  // N = 2 or 3 (point dimension), ND is the camera dimension
 | ||||
|   static SymmetricBlockMatrix SchurComplement( | ||||
|       const std::vector< | ||||
|           Eigen::Matrix<double, ZDim, ND>, | ||||
|           Eigen::aligned_allocator<Eigen::Matrix<double, ZDim, ND>>>& Fs, | ||||
|       const Matrix& E, const Eigen::Matrix<double, N, N>& P, const Vector& b) { | ||||
|     // a single point is observed in m cameras
 | ||||
|     size_t m = Fs.size(); | ||||
| 
 | ||||
|       // a single point is observed in m cameras
 | ||||
|       size_t m = Fs.size(); | ||||
|     // Create a SymmetricBlockMatrix (augmented hessian, with extra row/column with info vector)
 | ||||
|     size_t M1 = ND * m + 1; | ||||
|     std::vector<DenseIndex> dims(m + 1); // this also includes the b term
 | ||||
|     std::fill(dims.begin(), dims.end() - 1, ND); | ||||
|     dims.back() = 1; | ||||
|     SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1)); | ||||
| 
 | ||||
|       // Create a SymmetricBlockMatrix (augmented hessian, with extra row/column with info vector)
 | ||||
|       size_t M1 = ND * m + 1; | ||||
|       std::vector<DenseIndex> dims(m + 1); // this also includes the b term
 | ||||
|       std::fill(dims.begin(), dims.end() - 1, ND); | ||||
|       dims.back() = 1; | ||||
|       SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(M1, M1)); | ||||
|     // Blockwise Schur complement
 | ||||
|     for (size_t i = 0; i < m; i++) { // for each camera
 | ||||
| 
 | ||||
|       // Blockwise Schur complement
 | ||||
|       for (size_t i = 0; i < m; i++) { // for each camera
 | ||||
|       const Eigen::Matrix<double, ZDim, ND>& Fi = Fs[i]; | ||||
|       const auto FiT = Fi.transpose(); | ||||
|       const Eigen::Matrix<double, ZDim, N> Ei_P = //
 | ||||
|           E.block(ZDim * i, 0, ZDim, N) * P; | ||||
| 
 | ||||
|         const Eigen::Matrix<double, ZDim, ND>& Fi = Fs[i]; | ||||
|         const auto FiT = Fi.transpose(); | ||||
|         const Eigen::Matrix<double, ZDim, N> Ei_P = //
 | ||||
|             E.block(ZDim * i, 0, ZDim, N) * P; | ||||
|       // D = (Dx2) * ZDim
 | ||||
|       augmentedHessian.setOffDiagonalBlock(i, m, FiT * b.segment<ZDim>(ZDim * i) // F' * b
 | ||||
|       - FiT * (Ei_P * (E.transpose() * b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1)
 | ||||
| 
 | ||||
|         // D = (Dx2) * ZDim
 | ||||
|         augmentedHessian.setOffDiagonalBlock(i, m, FiT * b.segment<ZDim>(ZDim * i) // F' * b
 | ||||
|         - FiT * (Ei_P * (E.transpose() * b))); // D = (DxZDim) * (ZDimx3) * (N*ZDimm) * (ZDimm x 1)
 | ||||
|       // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
 | ||||
|       augmentedHessian.setDiagonalBlock(i, FiT | ||||
|           * (Fi - Ei_P * E.block(ZDim * i, 0, ZDim, N).transpose() * Fi)); | ||||
| 
 | ||||
|         // (DxD) = (DxZDim) * ( (ZDimxD) - (ZDimx3) * (3xZDim) * (ZDimxD) )
 | ||||
|         augmentedHessian.setDiagonalBlock(i, FiT | ||||
|             * (Fi - Ei_P * E.block(ZDim * i, 0, ZDim, N).transpose() * Fi)); | ||||
|       // upper triangular part of the hessian
 | ||||
|       for (size_t j = i + 1; j < m; j++) { // for each camera
 | ||||
|         const Eigen::Matrix<double, ZDim, ND>& Fj = Fs[j]; | ||||
| 
 | ||||
|         // upper triangular part of the hessian
 | ||||
|         for (size_t j = i + 1; j < m; j++) { // for each camera
 | ||||
|           const Eigen::Matrix<double, ZDim, ND>& Fj = Fs[j]; | ||||
|         // (DxD) = (Dx2) * ( (2x2) * (2xD) )
 | ||||
|         augmentedHessian.setOffDiagonalBlock(i, j, -FiT | ||||
|             * (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj)); | ||||
|       } | ||||
|     } // end of for over cameras
 | ||||
| 
 | ||||
|           // (DxD) = (Dx2) * ( (2x2) * (2xD) )
 | ||||
|           augmentedHessian.setOffDiagonalBlock(i, j, -FiT | ||||
|               * (Ei_P * E.block(ZDim * j, 0, ZDim, N).transpose() * Fj)); | ||||
|         } | ||||
|       } // end of for over cameras
 | ||||
| 
 | ||||
|       augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm(); | ||||
|       return augmentedHessian; | ||||
|     } | ||||
|     augmentedHessian.diagonalBlock(m)(0, 0) += b.squaredNorm(); | ||||
|     return augmentedHessian; | ||||
|   } | ||||
| 
 | ||||
|   /**
 | ||||
|    * Do Schur complement, given Jacobian as Fs,E,P, return SymmetricBlockMatrix | ||||
|    * G = F' * F - F' * E * P * E' * F | ||||
|    * g = F' * (b - E * P * E' * b) | ||||
|    * In this version, we allow for the case where the keys in the Jacobian are organized | ||||
|    * differently from the keys in the output SymmetricBlockMatrix | ||||
|    * In particular: each diagonal block of the Jacobian F captures 2 poses (useful for rolling shutter and extrinsic calibration) | ||||
|    * such that F keeps the block structure that makes the Schur complement trick fast. | ||||
|    * In this version, we allow for the case where the keys in the Jacobian are | ||||
|    * organized differently from the keys in the output SymmetricBlockMatrix In | ||||
|    * particular: each diagonal block of the Jacobian F captures 2 poses (useful | ||||
|    * for rolling shutter and extrinsic calibration) such that F keeps the block | ||||
|    * structure that makes the Schur complement trick fast. | ||||
|    * | ||||
|    * N = 2 or 3 (point dimension), ND is the Jacobian block dimension, NDD is | ||||
|    * the Hessian block dimension | ||||
|    */ | ||||
|   template<int N, int ND, int NDD>  // N = 2 or 3 (point dimension), ND is the Jacobian block dimension, NDD is the Hessian block dimension
 | ||||
|   template <int N, int ND, int NDD> | ||||
|   static SymmetricBlockMatrix SchurComplementAndRearrangeBlocks( | ||||
|       const std::vector<Eigen::Matrix<double, ZDim, ND>, | ||||
|           Eigen::aligned_allocator<Eigen::Matrix<double, ZDim, ND> > >& Fs, | ||||
|       const std::vector< | ||||
|           Eigen::Matrix<double, ZDim, ND>, | ||||
|           Eigen::aligned_allocator<Eigen::Matrix<double, ZDim, ND>>>& Fs, | ||||
|       const Matrix& E, const Eigen::Matrix<double, N, N>& P, const Vector& b, | ||||
|       const KeyVector jacobianKeys, const KeyVector hessianKeys) { | ||||
| 
 | ||||
|       const KeyVector& jacobianKeys, const KeyVector& hessianKeys) { | ||||
|     size_t nrNonuniqueKeys = jacobianKeys.size(); | ||||
|     size_t nrUniqueKeys = hessianKeys.size(); | ||||
| 
 | ||||
|  |  | |||
|  | @ -18,6 +18,7 @@ | |||
| #pragma once | ||||
| 
 | ||||
| #include <gtsam/slam/SmartProjectionFactor.h> | ||||
| #include <gtsam/geometry/CameraSet.h> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| /**
 | ||||
|  | @ -229,7 +230,9 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM | |||
|           keyPairsEqual = false; break; | ||||
|         } | ||||
|       } | ||||
|     }else{ keyPairsEqual = false; } | ||||
|     } else { | ||||
|       keyPairsEqual = false; | ||||
|     } | ||||
| 
 | ||||
|     double extrinsicCalibrationEqual = true; | ||||
|     if(this->body_P_sensors_.size() == e->body_P_sensors().size()){ | ||||
|  | @ -238,7 +241,9 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM | |||
|           extrinsicCalibrationEqual = false; break; | ||||
|         } | ||||
|       } | ||||
|     }else{ extrinsicCalibrationEqual = false; } | ||||
|     } else { | ||||
|       extrinsicCalibrationEqual = false; | ||||
|     } | ||||
| 
 | ||||
|     return e && Base::equals(p, tol) && K_all_ == e->calibration() | ||||
|         && alphas_ == e->alphas() && keyPairsEqual && extrinsicCalibrationEqual; | ||||
|  | @ -248,9 +253,10 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM | |||
|    * Compute jacobian F, E and error vector at a given linearization point | ||||
|    * @param values Values structure which must contain camera poses | ||||
|    * corresponding to keys involved in this factor | ||||
|    * @return Return arguments are the camera jacobians Fs (including the jacobian with | ||||
|    * respect to both body poses we interpolate from), the point Jacobian E, | ||||
|    * and the error vector b. Note that the jacobians are computed for a given point. | ||||
|    * @return Return arguments are the camera jacobians Fs (including the | ||||
|    * jacobian with respect to both body poses we interpolate from), the point | ||||
|    * Jacobian E, and the error vector b. Note that the jacobians are computed | ||||
|    * for a given point. | ||||
|    */ | ||||
|   void computeJacobiansWithTriangulatedPoint(FBlocks& Fs, Matrix& E, Vector& b, | ||||
|                                              const Values& values) const { | ||||
|  | @ -267,13 +273,15 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM | |||
|       Eigen::Matrix<double, ZDim, 3> Ei; | ||||
| 
 | ||||
|       for (size_t i = 0; i < numViews; i++) {  // for each camera/measurement
 | ||||
|         const Pose3& w_P_body1 = values.at<Pose3>(world_P_body_key_pairs_[i].first); | ||||
|         const Pose3& w_P_body2 = values.at<Pose3>(world_P_body_key_pairs_[i].second); | ||||
|         auto w_P_body1 = values.at<Pose3>(world_P_body_key_pairs_[i].first); | ||||
|         auto w_P_body2 = values.at<Pose3>(world_P_body_key_pairs_[i].second); | ||||
|         double interpolationFactor = alphas_[i]; | ||||
|         // get interpolated pose:
 | ||||
|         const Pose3& w_P_body = interpolate<Pose3>(w_P_body1, w_P_body2,interpolationFactor, dInterpPose_dPoseBody1, dInterpPose_dPoseBody2); | ||||
|         const Pose3& body_P_cam = body_P_sensors_[i]; | ||||
|         const Pose3& w_P_cam = w_P_body.compose(body_P_cam, dPoseCam_dInterpPose); | ||||
|         auto w_P_body = | ||||
|             interpolate<Pose3>(w_P_body1, w_P_body2, interpolationFactor, | ||||
|                                dInterpPose_dPoseBody1, dInterpPose_dPoseBody2); | ||||
|         auto body_P_cam = body_P_sensors_[i]; | ||||
|         auto w_P_cam = w_P_body.compose(body_P_cam, dPoseCam_dInterpPose); | ||||
|         PinholeCamera<CALIBRATION> camera(w_P_cam, *K_all_[i]); | ||||
| 
 | ||||
|         // get jacobians and error vector for current measurement
 | ||||
|  | @ -296,37 +304,40 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<CAM | |||
|   } | ||||
| 
 | ||||
|   /// linearize and return a Hessianfactor that is an approximation of error(p)
 | ||||
|   boost::shared_ptr<RegularHessianFactor<DimPose> > createHessianFactor( | ||||
|       const Values& values, const double lambda = 0.0, bool diagonalDamping = | ||||
|           false) const { | ||||
| 
 | ||||
|     // we may have multiple observation sharing the same keys (due to the rolling shutter interpolation),
 | ||||
|     // hence the number of unique keys may be smaller than 2 * nrMeasurements
 | ||||
|     size_t nrUniqueKeys = this->keys_.size(); // note: by construction, keys_ only contains unique keys
 | ||||
|   boost::shared_ptr<RegularHessianFactor<DimPose>> createHessianFactor( | ||||
|       const Values& values, const double lambda = 0.0, | ||||
|       bool diagonalDamping = false) const { | ||||
|     // we may have multiple observation sharing the same keys (due to the
 | ||||
|     // rolling shutter interpolation), hence the number of unique keys may be
 | ||||
|     // smaller than 2 * nrMeasurements
 | ||||
|     size_t nrUniqueKeys = | ||||
|         this->keys_ | ||||
|             .size();  // note: by construction, keys_ only contains unique keys
 | ||||
| 
 | ||||
|     // Create structures for Hessian Factors
 | ||||
|     KeyVector js; | ||||
|     std::vector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); | ||||
|     std::vector < Vector > gs(nrUniqueKeys); | ||||
|     std::vector<Matrix> Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); | ||||
|     std::vector<Vector> gs(nrUniqueKeys); | ||||
| 
 | ||||
|     if (this->measured_.size() != this->cameras(values).size()) // 1 observation per interpolated camera
 | ||||
|       throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: " | ||||
|                                "measured_.size() inconsistent with input"); | ||||
|     if (this->measured_.size() != | ||||
|         this->cameras(values).size())  // 1 observation per interpolated camera
 | ||||
|       throw std::runtime_error( | ||||
|           "SmartProjectionPoseFactorRollingShutter: " | ||||
|           "measured_.size() inconsistent with input"); | ||||
| 
 | ||||
|     // triangulate 3D point at given linearization point
 | ||||
|     this->triangulateSafe(this->cameras(values)); | ||||
| 
 | ||||
|     if (!this->result_) {  // failed: return "empty/zero" Hessian
 | ||||
|       if (this->params_.degeneracyMode == ZERO_ON_DEGENERACY) { | ||||
|         for (Matrix& m : Gs) | ||||
|           m = Matrix::Zero(DimPose, DimPose); | ||||
|         for (Vector& v : gs) | ||||
|           v = Vector::Zero(DimPose); | ||||
|         return boost::make_shared < RegularHessianFactor<DimPose> | ||||
|             > (this->keys_, Gs, gs, 0.0); | ||||
|         for (Matrix& m : Gs) m = Matrix::Zero(DimPose, DimPose); | ||||
|         for (Vector& v : gs) v = Vector::Zero(DimPose); | ||||
|         return boost::make_shared<RegularHessianFactor<DimPose>>(this->keys_, | ||||
|                                                                  Gs, gs, 0.0); | ||||
|       } else { | ||||
|         throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: " | ||||
|                                        "only supported degeneracy mode is ZERO_ON_DEGENERACY"); | ||||
|         throw std::runtime_error( | ||||
|             "SmartProjectionPoseFactorRollingShutter: " | ||||
|             "only supported degeneracy mode is ZERO_ON_DEGENERACY"); | ||||
|       } | ||||
|     } | ||||
|     // compute Jacobian given triangulated 3D Point
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue