test still failing
							parent
							
								
									e8db2b6b9b
								
							
						
					
					
						commit
						8b4a74efff
					
				|  | @ -192,12 +192,12 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { | |||
|       const Values& values, const double lambda = 0.0,  bool diagonalDamping = | ||||
|           false) const { | ||||
| 
 | ||||
|     size_t nrKeys = keys_.size(); | ||||
|     size_t nrUniqueKeys = keys_.size(); | ||||
| 
 | ||||
|     // Create structures for Hessian Factors
 | ||||
|     KeyVector js; | ||||
|     std::vector<Matrix> Gs(nrKeys * (nrKeys + 1) / 2); | ||||
|     std::vector<Vector> gs(nrKeys); | ||||
|     std::vector<Matrix> Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); | ||||
|     std::vector<Vector> gs(nrUniqueKeys); | ||||
| 
 | ||||
|     if (this->measured_.size() != cameras(values).size()) | ||||
|       throw std::runtime_error("SmartStereoProjectionHessianFactor: this->" | ||||
|  | @ -223,34 +223,28 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { | |||
|     computeJacobiansWithTriangulatedPoint(Fs, E, b, values); | ||||
| 
 | ||||
|     // Whiten using noise model
 | ||||
| //    std::cout << "noise model1  \n " << std::endl;
 | ||||
|     noiseModel_->WhitenSystem(E, b); | ||||
| //    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;
 | ||||
|     // 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;
 | ||||
|     // marginalize point
 | ||||
|     SymmetricBlockMatrix augmentedHessian = //
 | ||||
|         Cameras::SchurComplement<3,Dim>(Fs, E, P, b); | ||||
| 
 | ||||
|     std::vector<DenseIndex> dims(nrKeys + 1); // this also includes the b term
 | ||||
|     // now pack into an Hessian factor
 | ||||
|     std::vector<DenseIndex> dims(nrUniqueKeys + 1); // this also includes the b term
 | ||||
|     std::fill(dims.begin(), dims.end() - 1, 6); | ||||
|     dims.back() = 1; | ||||
| 
 | ||||
|     size_t nrNonuniqueKeys = w_P_body_keys_.size() + body_P_cam_keys_.size(); | ||||
|     SymmetricBlockMatrix augmentedHessianPP; | ||||
|     if ( nrKeys == nrNonuniqueKeys ){ // 1 calibration per camera
 | ||||
|       augmentedHessianPP = SymmetricBlockMatrix(dims, Matrix(augmentedHessian.selfadjointView())); | ||||
|     }else{ | ||||
|     SymmetricBlockMatrix augmentedHessianUniqueKeys; | ||||
|     if ( nrUniqueKeys == nrNonuniqueKeys ){ // if there is 1 calibration key per camera
 | ||||
|       augmentedHessianUniqueKeys = SymmetricBlockMatrix(dims, Matrix(augmentedHessian.selfadjointView())); | ||||
|     }else{ // if multiple cameras share a calibration
 | ||||
|       std::vector<DenseIndex> nonuniqueDims(nrNonuniqueKeys + 1); // this also includes the b term
 | ||||
|       std::fill(nonuniqueDims.begin(), nonuniqueDims.end() - 1, 6); | ||||
|       nonuniqueDims.back() = 1; | ||||
|  | @ -263,54 +257,55 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { | |||
|         nonuniqueKeys.push_back(body_P_cam_keys_.at(i)); | ||||
|       } | ||||
| 
 | ||||
|       // get map from key to location in the new augmented Hessian matrix
 | ||||
|       // get map from key to location in the new augmented Hessian matrix (the one including only unique keys)
 | ||||
|       std::map<Key,size_t> keyToSlotMap; | ||||
|       for(size_t k=0; k<nrKeys;k++){ | ||||
|       for(size_t k=0; k<nrUniqueKeys;k++){ | ||||
|         keyToSlotMap[keys_[k]] = k; | ||||
|       } | ||||
| 
 | ||||
|       std::cout << "linearize" << std::endl; | ||||
|       for(size_t i=0; i<nrKeys;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; | ||||
|       } | ||||
| //      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
 | ||||
|       augmentedHessianPP = SymmetricBlockMatrix(dims, Matrix::Zero(6*nrKeys+1,6*nrKeys+1)); | ||||
|       augmentedHessianUniqueKeys = SymmetricBlockMatrix(dims, Matrix::Zero(6*nrUniqueKeys+1,6*nrUniqueKeys+1)); | ||||
| 
 | ||||
|       std::cout <<"  start for loop: " << std::endl; | ||||
|       // add contributions for each key: note this loops over the hessian with nonUnique keys (augmentedHessian)
 | ||||
|       for(size_t i=0; i<nrNonuniqueKeys;i++){ // rows
 | ||||
|         Key key_i = nonuniqueKeys.at(i); | ||||
|         std::cout <<"  start for loop i: " << std::endl; | ||||
| 
 | ||||
|         // update information vector
 | ||||
|         augmentedHessianUniqueKeys.updateOffDiagonalBlock( keyToSlotMap[key_i] , nrUniqueKeys, | ||||
|                                                                augmentedHessian.aboveDiagonalBlock(i,nrNonuniqueKeys)); | ||||
| 
 | ||||
|         // update blocks
 | ||||
|         for(size_t j=0; j<nrNonuniqueKeys;j++){ // cols
 | ||||
|           std::cout <<"  start for loop j: " << std::endl; | ||||
|           Key key_j = nonuniqueKeys.at(j); | ||||
|           std::cout <<"key_i: " << DefaultKeyFormatter(key_i); | ||||
|           std::cout <<" key_j: " << DefaultKeyFormatter(key_j); | ||||
|           std::cout <<"  start for loop --: " << std::endl; | ||||
|           if(i==j){ | ||||
|             std::cout <<"  i=0: " << std::endl; | ||||
|             augmentedHessianPP.updateDiagonalBlock( keyToSlotMap[key_i] , augmentedHessian.diagonalBlock(i)); | ||||
|             augmentedHessianUniqueKeys.updateDiagonalBlock( keyToSlotMap[key_i] , augmentedHessian.diagonalBlock(i)); | ||||
|           }else if(i < j){ | ||||
|             std::cout <<"  i<j: " << std::endl; | ||||
|             augmentedHessianPP.updateOffDiagonalBlock( keyToSlotMap[key_i] , keyToSlotMap[key_j], | ||||
|             augmentedHessianUniqueKeys.updateOffDiagonalBlock( keyToSlotMap[key_i] , keyToSlotMap[key_j], | ||||
|                                                        augmentedHessian.aboveDiagonalBlock(i,j)); | ||||
|           } | ||||
|           else{ | ||||
|             std::cout <<"  i>j: " << std::endl; | ||||
|             augmentedHessianPP.updateOffDiagonalBlock( keyToSlotMap[key_i] , keyToSlotMap[key_j], | ||||
|                                                        augmentedHessian.aboveDiagonalBlock(j,i)); | ||||
|             augmentedHessianUniqueKeys.updateOffDiagonalBlock( keyToSlotMap[key_i] , keyToSlotMap[key_j], | ||||
|                                                        augmentedHessian.aboveDiagonalBlock(j,i).transpose()); | ||||
|           } | ||||
|         } | ||||
|       } | ||||
|       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_, augmentedHessianPP); | ||||
| 
 | ||||
|     return boost::make_shared<RegularHessianFactor<DimPose> >(keys_, augmentedHessianUniqueKeys); | ||||
|     //std::cout << "Matrix(augmentedHessian.selfadjointView()) \n" << Matrix(augmentedHessian.selfadjointView()) <<std::endl;
 | ||||
|   } | ||||
| 
 | ||||
|  |  | |||
|  | @ -609,6 +609,142 @@ TEST( SmartStereoProjectionFactorPP, 3poses_smart_projection_factor_error_sameEx | |||
|   EXPECT_DOUBLES_EQUAL(0.0, initialErrorSmart, 1e-5); // initial guess is noisy, so nonzero error
 | ||||
| } | ||||
| 
 | ||||
| /* *************************************************************************/ | ||||
| TEST( SmartStereoProjectionFactorPP, 3poses_smart_projection_factor_error_sameExtrinsicKey_error_noisy ) { | ||||
| 
 | ||||
|   // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
 | ||||
|   Pose3 w_Pose_cam1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1)); | ||||
|   StereoCamera cam1(w_Pose_cam1, K2); | ||||
| 
 | ||||
|   // create second camera 1 meter to the right of first camera
 | ||||
|   Pose3 w_Pose_cam2 = w_Pose_cam1 * Pose3(Rot3(), Point3(1, 0, 0)); | ||||
|   StereoCamera cam2(w_Pose_cam2, K2); | ||||
| 
 | ||||
|   // create third camera 1 meter above the first camera
 | ||||
|   Pose3 w_Pose_cam3 = w_Pose_cam1 * Pose3(Rot3(), Point3(0, -1, 0)); | ||||
|   StereoCamera cam3(w_Pose_cam3, K2); | ||||
| 
 | ||||
|   // three landmarks ~5 meters infront of camera
 | ||||
|   Point3 landmark1(5, 0.5, 1.2); | ||||
|   Point3 landmark2(5, -0.5, 1.2); | ||||
|   Point3 landmark3(3, 0, 3.0); | ||||
| 
 | ||||
|   // 1. Project three landmarks into three cameras and triangulate
 | ||||
|   vector<StereoPoint2> measurements_l1 = stereo_projectToMultipleCameras(cam1, | ||||
|                                                                          cam2, cam3, landmark1); | ||||
|   vector<StereoPoint2> measurements_l2 = stereo_projectToMultipleCameras(cam1, | ||||
|                                                                          cam2, cam3, landmark2); | ||||
|   vector<StereoPoint2> measurements_l3 = stereo_projectToMultipleCameras(cam1, | ||||
|                                                                          cam2, cam3, landmark3); | ||||
| 
 | ||||
|   double initialError_expected, initialError_actual; | ||||
|   { | ||||
|     KeyVector poseKeys; | ||||
|     poseKeys.push_back(x1); | ||||
|     poseKeys.push_back(x2); | ||||
|     poseKeys.push_back(x3); | ||||
| 
 | ||||
|     KeyVector extrinsicKeys; | ||||
|     extrinsicKeys.push_back(body_P_cam1_key); | ||||
|     extrinsicKeys.push_back(body_P_cam2_key); | ||||
|     extrinsicKeys.push_back(body_P_cam3_key); | ||||
| 
 | ||||
|     SmartStereoProjectionParams smart_params; | ||||
|     smart_params.triangulation.enableEPI = true; | ||||
|     SmartStereoProjectionFactorPP::shared_ptr smartFactor1(new SmartStereoProjectionFactorPP(model, smart_params)); | ||||
|     smartFactor1->add(measurements_l1, poseKeys, extrinsicKeys, K2); | ||||
| 
 | ||||
|     SmartStereoProjectionFactorPP::shared_ptr smartFactor2(new SmartStereoProjectionFactorPP(model, smart_params)); | ||||
|     smartFactor2->add(measurements_l2, poseKeys, extrinsicKeys, K2); | ||||
| 
 | ||||
|     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
 | ||||
|     Pose3 body_Pose_cam1 = Pose3(Rot3::Ypr(-M_PI, 1., 0.1),Point3(0, 1, 0)); | ||||
|     Pose3 body_Pose_cam2 = body_Pose_cam1; | ||||
|     Pose3 body_Pose_cam3 = body_Pose_cam1; | ||||
|     Pose3 w_Pose_body1 = w_Pose_cam1.compose(body_Pose_cam1.inverse()); | ||||
|     Pose3 w_Pose_body2 = w_Pose_cam2.compose(body_Pose_cam2.inverse()); | ||||
|     Pose3 w_Pose_body3 = w_Pose_cam3.compose(body_Pose_cam3.inverse()); | ||||
| 
 | ||||
|     Values values; | ||||
|     Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise
 | ||||
|     values.insert(x1, w_Pose_body1); | ||||
|     values.insert(x2, w_Pose_body2); | ||||
|     values.insert(x3, w_Pose_body3); | ||||
|     values.insert(body_P_cam1_key, body_Pose_cam1 * noise_pose); | ||||
|     values.insert(body_P_cam2_key, body_Pose_cam2 * noise_pose); | ||||
|     // initialize third calibration with some noise, we expect it to move back to original pose3
 | ||||
|     values.insert(body_P_cam3_key, body_Pose_cam3 * noise_pose); | ||||
| 
 | ||||
|     // Graph
 | ||||
|     NonlinearFactorGraph graph; | ||||
|     graph.push_back(smartFactor1); | ||||
|     graph.push_back(smartFactor2); | ||||
|     graph.push_back(smartFactor3); | ||||
|     graph.addPrior(x1, w_Pose_body1, noisePrior); | ||||
|     graph.addPrior(x2, w_Pose_body2, noisePrior); | ||||
|     graph.addPrior(x3, w_Pose_body3, noisePrior); | ||||
| 
 | ||||
|     initialError_expected = graph.error(values); | ||||
|   } | ||||
| 
 | ||||
|   { | ||||
|     KeyVector poseKeys; | ||||
|     poseKeys.push_back(x1); | ||||
|     poseKeys.push_back(x2); | ||||
|     poseKeys.push_back(x3); | ||||
| 
 | ||||
|     KeyVector extrinsicKeys; | ||||
|     extrinsicKeys.push_back(body_P_cam1_key); | ||||
|     extrinsicKeys.push_back(body_P_cam1_key); | ||||
|     extrinsicKeys.push_back(body_P_cam1_key); | ||||
| 
 | ||||
|     SmartStereoProjectionParams smart_params; | ||||
|     smart_params.triangulation.enableEPI = true; | ||||
|     SmartStereoProjectionFactorPP::shared_ptr smartFactor1(new SmartStereoProjectionFactorPP(model, smart_params)); | ||||
|     smartFactor1->add(measurements_l1, poseKeys, extrinsicKeys, K2); | ||||
| 
 | ||||
|     SmartStereoProjectionFactorPP::shared_ptr smartFactor2(new SmartStereoProjectionFactorPP(model, smart_params)); | ||||
|     smartFactor2->add(measurements_l2, poseKeys, extrinsicKeys, K2); | ||||
| 
 | ||||
|     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
 | ||||
|     Pose3 body_Pose_cam1 = Pose3(Rot3::Ypr(-M_PI, 1., 0.1),Point3(0, 1, 0)); | ||||
|     Pose3 w_Pose_body1 = w_Pose_cam1.compose(body_Pose_cam1.inverse()); | ||||
|     Pose3 w_Pose_body2 = w_Pose_cam2.compose(body_Pose_cam1.inverse()); | ||||
|     Pose3 w_Pose_body3 = w_Pose_cam3.compose(body_Pose_cam1.inverse()); | ||||
| 
 | ||||
|     Values values; | ||||
|     Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.1, 0.1, 0.1)); // smaller noise
 | ||||
|     values.insert(x1, w_Pose_body1); | ||||
|     values.insert(x2, w_Pose_body2); | ||||
|     values.insert(x3, w_Pose_body3); | ||||
|     values.insert(body_P_cam1_key, body_Pose_cam1 * noise_pose); | ||||
| 
 | ||||
|     // Graph
 | ||||
|     NonlinearFactorGraph graph; | ||||
|     graph.push_back(smartFactor1); | ||||
|     graph.push_back(smartFactor2); | ||||
|     graph.push_back(smartFactor3); | ||||
|     graph.addPrior(x1, w_Pose_body1, noisePrior); | ||||
|     graph.addPrior(x2, w_Pose_body2, noisePrior); | ||||
|     graph.addPrior(x3, w_Pose_body3, noisePrior); | ||||
| 
 | ||||
|     initialError_actual = graph.error(values); | ||||
|   } | ||||
| 
 | ||||
|   std::cout << " initialError_expected " << initialError_expected << std::endl; | ||||
|   EXPECT_DOUBLES_EQUAL(initialError_expected, initialError_actual, 1e-7); | ||||
| } | ||||
| 
 | ||||
| /* *************************************************************************/ | ||||
| TEST( SmartStereoProjectionFactorPP, 3poses_smart_projection_factor_optimization_sameExtrinsicKey ) { | ||||
| 
 | ||||
|  | @ -689,6 +825,10 @@ TEST( SmartStereoProjectionFactorPP, 3poses_smart_projection_factor_optimization | |||
|   double initialErrorSmart = graph.error(values); | ||||
|   EXPECT_DOUBLES_EQUAL(31986.961831653316, initialErrorSmart, 1e-5); // initial guess is noisy, so nonzero error
 | ||||
| 
 | ||||
|   std::cout << " 1: " << smartFactor1->error(values) <<std::endl; | ||||
|   std::cout << " 2: " << smartFactor2->error(values) <<std::endl; | ||||
|   std::cout << " 3: " << smartFactor3->error(values) <<std::endl; | ||||
| 
 | ||||
|   Values result; | ||||
|   gttic_(SmartStereoProjectionFactorPP); | ||||
|   LevenbergMarquardtOptimizer optimizer(graph, values, lm_params); | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue