need to clean up templates and remove 2 redundant lines
							parent
							
								
									2e1ed2c852
								
							
						
					
					
						commit
						5677bdb6c1
					
				|  | @ -449,10 +449,10 @@ public: | |||
|   /**
 | ||||
|    * This corrects the Jacobians and error vector for the case in which the right pixel in the monocular camera is missing (nan) | ||||
|    */ | ||||
|   template<int D, int ZD> // D: camera dim, ZD: measurement dim
 | ||||
|   void correctForMissingMeasurements(const Cameras& cameras, Vector& ue, | ||||
|       boost::optional<typename Cameras::FBlocks&> Fs = boost::none, | ||||
|       boost::optional<Matrix&> E = boost::none) const { | ||||
|                                      boost::optional<typename Cameras::FBlocks&> Fs = boost::none, | ||||
|                                      boost::optional<Matrix&> E = boost::none) const override | ||||
|   { | ||||
|     // when using stereo cameras, some of the measurements might be missing:
 | ||||
|     for(size_t i=0; i < cameras.size(); i++){ | ||||
|       const StereoPoint2& z = measured_.at(i); | ||||
|  | @ -460,28 +460,18 @@ public: | |||
|       { | ||||
|         if(Fs){ // delete influence of right point on jacobian Fs
 | ||||
|           MatrixZD& Fi = Fs->at(i); | ||||
|           for(size_t ii=0; ii<D; ii++) | ||||
|           for(size_t ii=0; ii<Dim; ii++) | ||||
|             Fi(1,ii) = 0.0; | ||||
|         } | ||||
|         if(E) // delete influence of right point on jacobian E
 | ||||
|           E->row(ZD * i + 1) = Matrix::Zero(1, E->cols()); | ||||
|           E->row(ZDim * i + 1) = Matrix::Zero(1, E->cols()); | ||||
| 
 | ||||
|         // set the corresponding entry of vector ue to zero
 | ||||
|         ue(ZD * i + 1) = 0.0; | ||||
|         ue(ZDim * i + 1) = 0.0; | ||||
|       } | ||||
|     } | ||||
|   } | ||||
| 
 | ||||
|   /**
 | ||||
|    * This corrects the Jacobians and error vector for the case in which the right pixel in the monocular camera is missing (nan) | ||||
|    * This is class implementation that directly uses the measurement and camera size without templates. | ||||
|    */ | ||||
|   void correctForMissingMeasurements(const Cameras& cameras, Vector& ue, | ||||
|       boost::optional<typename Cameras::FBlocks&> Fs = boost::none, | ||||
|       boost::optional<Matrix&> E = boost::none) const override { | ||||
|     correctForMissingMeasurements<Dim,ZDim>(cameras, ue, Fs, E); | ||||
|   } | ||||
| 
 | ||||
|   /** return the landmark */ | ||||
|     TriangulationResult point() const { | ||||
|       return result_; | ||||
|  |  | |||
|  | @ -152,7 +152,7 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { | |||
|   /// Compute F, E only (called below in both vanilla and SVD versions)
 | ||||
|   /// Assumes the point has been computed
 | ||||
|   /// Note E can be 2m*3 or 2m*2, in case point is degenerate
 | ||||
|   void computeJacobiansWithTriangulatedPoint( | ||||
|   void computeJacobiansAndCorrectForMissingMeasurements( | ||||
|       FBlocks& Fs, | ||||
|       Matrix& E, Vector& b, const Values& values) const { | ||||
|     if (!result_) { | ||||
|  | @ -168,23 +168,20 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { | |||
|         Pose3 body_P_cam = values.at<Pose3>(body_P_cam_keys_.at(i)); | ||||
|         StereoCamera camera(w_P_body.compose(body_P_cam, dPoseCam_dPoseBody, dPoseCam_dPoseExt), K_all_[i]); | ||||
|         StereoPoint2 reprojectionError = StereoPoint2(camera.project(*result_, dProject_dPoseCam, Ei) - measured_.at(i)); | ||||
| //        std::cout << "H0 \n" << dPoseCam_dPoseBody << std::endl;
 | ||||
| //        std::cout << "H1 \n" << dProject_dPoseCam << std::endl;
 | ||||
| //        std::cout << "H3 \n" << Ei << std::endl;
 | ||||
| //        std::cout << "H02 \n" << dPoseCam_dPoseExt << std::endl;
 | ||||
|         Eigen::Matrix<double, ZDim, Dim> J; // 3 x 12
 | ||||
| //        std::cout << "H1 * H0 \n" << dProject_dPoseCam * dPoseCam_dPoseBody << std::endl;
 | ||||
| //        std::cout << "H1 * H02 \n" << dProject_dPoseCam * dPoseCam_dPoseExt << std::endl;
 | ||||
|         J.block<ZDim,6>(0,0) = dProject_dPoseCam * dPoseCam_dPoseBody; // (3x6) * (6x6)
 | ||||
|         J.block<ZDim,6>(0,6) = dProject_dPoseCam * dPoseCam_dPoseExt; // (3x6) * (6x6)
 | ||||
| //        std::cout << "J \n" << J << std::endl;
 | ||||
|         if(std::isnan(measured_.at(i).uR())) // if the right pixel is invalid
 | ||||
|         { | ||||
|           J.block<1,12>(1,0) = Matrix::Zero(1,12); | ||||
|           Ei.block<1,3>(1,0) = Matrix::Zero(1,3); | ||||
|           reprojectionError = StereoPoint2(reprojectionError.uL(), 0.0, reprojectionError.v() ); | ||||
|         } | ||||
|         Fs.push_back(J); | ||||
|         size_t row = 3*i; | ||||
|         b.segment<ZDim>(row) = - reprojectionError.vector(); | ||||
|         E.block<3,3>(row,0) = Ei; | ||||
|       } | ||||
|       // correct for monocular measurements, where the right pixel measurement is nan
 | ||||
|       //Base::CorrectForMissingMeasurements(measured_, cameras, b, Fs, E);
 | ||||
|     } | ||||
|   } | ||||
| 
 | ||||
|  | @ -204,11 +201,9 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { | |||
|       throw std::runtime_error("SmartStereoProjectionHessianFactor: this->" | ||||
|           "measured_.size() inconsistent with input"); | ||||
| 
 | ||||
|     std::cout << "triangulate" << std::endl; | ||||
|     triangulateSafe(cameras(values)); | ||||
| 
 | ||||
|     if (!result_) { | ||||
|       std::cout << "degenerate" << std::endl; | ||||
|       // failed: return"empty" Hessian
 | ||||
|       for(Matrix& m: Gs) | ||||
|         m = Matrix::Zero(DimPose,DimPose); | ||||
|  | @ -218,12 +213,11 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { | |||
|                                                                   Gs, gs, 0.0); | ||||
|     } | ||||
| 
 | ||||
|     std::cout << "params_.degeneracyMode" << params_.degeneracyMode <<  std::endl; | ||||
|     // Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols().
 | ||||
|     FBlocks Fs; | ||||
|     Matrix F, E; | ||||
|     Vector b; | ||||
|     computeJacobiansWithTriangulatedPoint(Fs, E, b, values); | ||||
|     computeJacobiansAndCorrectForMissingMeasurements(Fs, E, b, values); | ||||
| 
 | ||||
|     // Whiten using noise model
 | ||||
|     noiseModel_->WhitenSystem(E, b); | ||||
|  | @ -266,16 +260,6 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { | |||
|         keyToSlotMap[keys_[k]] = k; | ||||
|       } | ||||
| 
 | ||||
|       std::cout << "linearize" << std::endl; | ||||
| //      for(size_t i=0; i<nrUniqueKeys;i++){
 | ||||
| //        std::cout <<"key: " << DefaultKeyFormatter(keys_[i]);
 | ||||
| //        std::cout <<"  key slot: " << keyToSlotMap[keys_[i]] << std::endl;
 | ||||
| //      }
 | ||||
| //      for(size_t i=0; i<nrNonuniqueKeys;i++){
 | ||||
| //        std::cout <<"key: " << DefaultKeyFormatter(nonuniqueKeys[i]);
 | ||||
| //        std::cout <<"  key slot: " << keyToSlotMap[nonuniqueKeys[i]] << std::endl;
 | ||||
| //      }
 | ||||
| 
 | ||||
|       // initialize matrix to zero
 | ||||
|       augmentedHessianUniqueKeys = SymmetricBlockMatrix(dims, Matrix::Zero(6*nrUniqueKeys+1,6*nrUniqueKeys+1)); | ||||
| 
 | ||||
|  | @ -309,13 +293,9 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { | |||
|         } | ||||
|       } | ||||
|       augmentedHessianUniqueKeys.updateDiagonalBlock(nrUniqueKeys, augmentedHessian.diagonalBlock(nrNonuniqueKeys)); | ||||
| 
 | ||||
|       //std::cout << "Matrix \n " << Matrix(augmentedHessianUniqueKeys.selfadjointView()) <<std::endl;
 | ||||
|       //std::cout << "sq norm " << b.squaredNorm() << std::endl;
 | ||||
|     } | ||||
| 
 | ||||
|     return boost::make_shared<RegularHessianFactor<DimPose> >(keys_, augmentedHessianUniqueKeys); | ||||
|     //std::cout << "Matrix(augmentedHessian.selfadjointView()) \n" << Matrix(augmentedHessian.selfadjointView()) <<std::endl;
 | ||||
|   } | ||||
| 
 | ||||
|   /**
 | ||||
|  |  | |||
|  | @ -862,7 +862,7 @@ TEST( SmartStereoProjectionFactorPP, 3poses_optimization_sameExtrinsicKey ) { | |||
|   EXPECT(assert_equal(expected, delta, 1e-4)); | ||||
| } | ||||
| 
 | ||||
| /* *************************************************************************
 | ||||
| /* *************************************************************************/ | ||||
| TEST( SmartStereoProjectionFactorPP, monocular_multipleExtrinsicKeys ){ | ||||
|   // make a realistic calibration matrix
 | ||||
|   double fov = 60; // degrees
 | ||||
|  | @ -979,7 +979,7 @@ TEST( SmartStereoProjectionFactorPP, monocular_multipleExtrinsicKeys ){ | |||
|   tictoc_finishedIteration_(); | ||||
| 
 | ||||
|   EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-5); | ||||
|   EXPECT(assert_equal(sensor_to_body,result.at<Pose3>(body_P_cam_key))); | ||||
|   EXPECT(assert_equal(sensor_to_body,result.at<Pose3>(body_P_cam_key), 1e-1)); | ||||
| } | ||||
| 
 | ||||
| /* *************************************************************************/ | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue