fixed print, removed cout, test still failing
							parent
							
								
									4df78be0f0
								
							
						
					
					
						commit
						7c052ff48a
					
				|  | @ -76,7 +76,7 @@ void SmartStereoProjectionFactorPP::add( | |||
| 
 | ||||
| void SmartStereoProjectionFactorPP::print( | ||||
|     const std::string& s, const KeyFormatter& keyFormatter) const { | ||||
|   std::cout << s << "SmartStereoProjectionFactorPP, z = \n "; | ||||
|   std::cout << s << "SmartStereoProjectionFactorPP: \n "; | ||||
|   for (size_t i = 0; i < K_all_.size(); i++) { | ||||
|     K_all_[i]->print("calibration = "); | ||||
|     std::cout << " extrinsic pose key: " << keyFormatter(body_P_cam_keys_[i]) << std::endl; | ||||
|  |  | |||
|  | @ -169,16 +169,16 @@ 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; | ||||
| //        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; | ||||
| //        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; | ||||
| //        std::cout << "J \n" << J << std::endl;
 | ||||
|         Fs.push_back(J); | ||||
|         size_t row = 3*i; | ||||
|         b.segment<ZDim>(row) = - reprojectionError.vector(); | ||||
|  | @ -205,7 +205,6 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { | |||
|     std::vector<Matrix> Gs(numKeys * (numKeys + 1) / 2); | ||||
|     std::vector<Vector> gs(numKeys); | ||||
| 
 | ||||
|     std::cout <<"using my hessian!!!!!!!!!!1" << std::endl; | ||||
|     for(size_t i=0; i<numKeys;i++){ | ||||
|       std::cout <<"key: " << DefaultKeyFormatter(allKeys[i]) << std::endl; | ||||
|     } | ||||
|  | @ -215,9 +214,9 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { | |||
|           "measured_.size() inconsistent with input"); | ||||
| 
 | ||||
|     triangulateSafe(cameras(values)); | ||||
|     std::cout <<"passed triangulateSafe!!!!!!!!!!1" << std::endl; | ||||
| 
 | ||||
|     if (params_.degeneracyMode == ZERO_ON_DEGENERACY && !result_) { | ||||
|       std::cout << "degenerate" << std::endl; | ||||
|       // failed: return"empty" Hessian
 | ||||
|       for(Matrix& m: Gs) | ||||
|         m = Matrix::Zero(DimPose,DimPose); | ||||
|  | @ -226,48 +225,46 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { | |||
|       return boost::make_shared<RegularHessianFactor<DimPose> >(allKeys, | ||||
|                                                                   Gs, gs, 0.0); | ||||
|     } | ||||
| 
 | ||||
| //    std::cout << "result_" << *result_ << std::endl;
 | ||||
| //    std::cout << "result_2" << result_ << std::endl;
 | ||||
|     // Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols().
 | ||||
|     FBlocks Fs; | ||||
|     Matrix F, E; | ||||
|     Vector b; | ||||
|     std::cout <<"before computeJacobiansWithTriangulatedPoint!!!!!!!!!!1" << std::endl; | ||||
|     computeJacobiansWithTriangulatedPoint(Fs, E, b, values); | ||||
|     std::cout << "Fs.at(0) \n"<< Fs.at(0) << std::endl; | ||||
| 
 | ||||
|     std::cout << "Dim "<< Dim << std::endl; | ||||
|     std::cout << "numKeys "<< numKeys << std::endl; | ||||
| 
 | ||||
|     std::cout << "Fs.size()  = " << Fs.size() << std::endl; | ||||
|     std::cout << "E  = " << E << std::endl; | ||||
|     std::cout << "b  = " << b << std::endl; | ||||
| //    std::cout << "Dim "<< Dim << std::endl;
 | ||||
| //    std::cout << "numKeys "<< numKeys << std::endl;
 | ||||
| //
 | ||||
| //    std::cout << "Fs.size()  = " << Fs.size() << std::endl;
 | ||||
| //    std::cout << "E  = " << E << std::endl;
 | ||||
| //    std::cout << "b  = " << b << std::endl;
 | ||||
| 
 | ||||
|     // Whiten using noise model
 | ||||
|     std::cout << "noise model1  \n " << std::endl; | ||||
| //    std::cout << "noise model1  \n " << std::endl;
 | ||||
|     noiseModel_->WhitenSystem(E, b); | ||||
|     std::cout << "noise model2  \n " << std::endl; | ||||
| //    std::cout << "noise model2  \n " << std::endl;
 | ||||
|     for (size_t i = 0; i < Fs.size(); i++) | ||||
|       Fs[i] = noiseModel_->Whiten(Fs[i]); | ||||
| 
 | ||||
|     std::cout << "noise model3  \n " << std::endl; | ||||
| //    std::cout << "noise model3  \n " << std::endl;
 | ||||
|     // build augmented hessian
 | ||||
|     Matrix3 P; | ||||
|     Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping); | ||||
| 
 | ||||
|     std::cout << "ComputePointCovariance done!!!  \n " << std::endl; | ||||
|     std::cout << "Fs.size()  = " << Fs.size() << std::endl; | ||||
|     std::cout << "E  = " << E << std::endl; | ||||
|     std::cout << "P  = " << P << std::endl; | ||||
|     std::cout << "b  = " << b << std::endl; | ||||
| //    std::cout << "ComputePointCovariance done!!!  \n " << std::endl;
 | ||||
| //    std::cout << "Fs.size()  = " << Fs.size() << std::endl;
 | ||||
| //    std::cout << "E  = " << E << std::endl;
 | ||||
| //    std::cout << "P  = " << P << std::endl;
 | ||||
| //    std::cout << "b  = " << b << std::endl;
 | ||||
|     SymmetricBlockMatrix augmentedHessian = //
 | ||||
|         Cameras::SchurComplement<3,Dim>(Fs, E, P, b); | ||||
| 
 | ||||
|     std::cout << "Repackaging!!!  \n " << std::endl; | ||||
| 
 | ||||
|     std::vector<DenseIndex> dims(numKeys + 1); // this also includes the b term
 | ||||
|     std::fill(dims.begin(), dims.end() - 1, 6); | ||||
|     dims.back() = 1; | ||||
|     SymmetricBlockMatrix augmentedHessianPP(dims, Matrix(augmentedHessian.selfadjointView())); | ||||
|     std::cout << "Matrix(augmentedHessian.selfadjointView()) \n" << Matrix(augmentedHessian.selfadjointView()) <<std::endl; | ||||
| 
 | ||||
|     return boost::make_shared<RegularHessianFactor<DimPose> >(allKeys, | ||||
|                                                           augmentedHessianPP); | ||||
|  |  | |||
|  | @ -659,22 +659,14 @@ TEST( SmartStereoProjectionFactorPP, 3poses_smart_projection_factor_optimization | |||
|   SmartStereoProjectionFactorPP::shared_ptr smartFactor3(new SmartStereoProjectionFactorPP(model, smart_params)); | ||||
|   smartFactor3->add(measurements_l3, poseKeys, extrinsicKeys, K2); | ||||
| 
 | ||||
|   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); | ||||
| 
 | ||||
|   // Values
 | ||||
|   // relevant poses:
 | ||||
|   Pose3 body_Pose_cam = Pose3(Rot3::Ypr(-M_PI, 1., 0.1),Point3(0, 1, 0)); | ||||
|   Pose3 w_Pose_body1 = w_Pose_cam1.compose(body_Pose_cam.inverse()); | ||||
|   Pose3 w_Pose_body2 = w_Pose_cam2.compose(body_Pose_cam.inverse()); | ||||
|   Pose3 w_Pose_body3 = w_Pose_cam3.compose(body_Pose_cam.inverse()); | ||||
| 
 | ||||
|   Values values; | ||||
|   Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.01, 0.01, 0.01)); // smaller noise
 | ||||
|   values.insert(x1, w_Pose_body1); | ||||
|   values.insert(x2, w_Pose_body2); | ||||
|   values.insert(x3, w_Pose_body3); | ||||
|   values.insert(body_P_cam_key, body_Pose_cam*noise_pose); | ||||
| 
 | ||||
|   // Graph
 | ||||
|   const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); | ||||
|   NonlinearFactorGraph graph; | ||||
|   graph.push_back(smartFactor1); | ||||
|   graph.push_back(smartFactor2); | ||||
|  | @ -683,7 +675,15 @@ TEST( SmartStereoProjectionFactorPP, 3poses_smart_projection_factor_optimization | |||
|   graph.addPrior(x2, w_Pose_body2, noisePrior); | ||||
|   graph.addPrior(x3, w_Pose_body3, noisePrior); | ||||
|   // we might need some prior on the calibration too
 | ||||
|   // graph.addPrior(body_P_cam_key, body_Pose_cam, noisePrior); // no need! the measurements will fix this!
 | ||||
|   graph.addPrior(body_P_cam_key, body_Pose_cam, noisePrior); // no need! the measurements will fix this!
 | ||||
| 
 | ||||
|   // Values
 | ||||
|   Values values; | ||||
|   Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.01, 0.01, 0.01)); // smaller noise
 | ||||
|   values.insert(x1, w_Pose_body1); | ||||
|   values.insert(x2, w_Pose_body2); | ||||
|   values.insert(x3, w_Pose_body3); | ||||
|   values.insert(body_P_cam_key, body_Pose_cam*noise_pose); | ||||
| 
 | ||||
|   // cost is large before optimization
 | ||||
|   double initialErrorSmart = graph.error(values); | ||||
|  | @ -697,6 +697,12 @@ TEST( SmartStereoProjectionFactorPP, 3poses_smart_projection_factor_optimization | |||
|   tictoc_finishedIteration_(); | ||||
| 
 | ||||
|   EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-5); | ||||
| 
 | ||||
|   GaussianFactorGraph::shared_ptr GFG = graph.linearize(result); | ||||
|   GFG->print("GFG \n"); | ||||
|   VectorValues delta = GFG->optimize(); | ||||
|   VectorValues expected = VectorValues::Zero(delta); | ||||
|   EXPECT(assert_equal(expected, delta, 1e-6)); | ||||
| } | ||||
| 
 | ||||
| /* *************************************************************************
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue