added computePoints functionality in smartFactorsCreator
							parent
							
								
									92f0bb64b2
								
							
						
					
					
						commit
						a1402018ca
					
				|  | @ -682,7 +682,6 @@ bool writeBALfromValues(const string& filename, SfM_data &data, Values& values){ | |||
|   if( valuesPoints.size() != data.number_tracks()){ | ||||
|     cout << "writeBALfromValues: different number of points in SfM_data (#points= " << data.number_tracks() | ||||
|         <<") and values (#points " << valuesPoints.size() << ")!!" << endl; | ||||
|     return false; | ||||
|   } | ||||
|   if(valuesPoints.size() + valuesPoses.size() != values.size()){ | ||||
|     cout << "writeBALfromValues write only poses and points values!!" << endl; | ||||
|  | @ -703,8 +702,15 @@ bool writeBALfromValues(const string& filename, SfM_data &data, Values& values){ | |||
| 
 | ||||
|   for (size_t j = 0; j < data.number_tracks(); j++){ // for each point
 | ||||
|     Key pointKey = symbol('l',j); | ||||
|     Point3 point = values.at<Point3>(pointKey); | ||||
|     data.tracks[j].p = point; | ||||
|     if(values.exists(pointKey)){ | ||||
|       Point3 point = values.at<Point3>(pointKey); | ||||
|       data.tracks[j].p = point; | ||||
|     }else{ | ||||
|       data.tracks[j].r = 1.0; | ||||
|       data.tracks[j].g = 0.0; | ||||
|       data.tracks[j].b = 0.0; | ||||
|       data.tracks[j].p = Point3(); | ||||
|     } | ||||
|   } | ||||
| 
 | ||||
|   return writeBAL(filename, data); | ||||
|  |  | |||
|  | @ -370,6 +370,10 @@ int main(int argc, char** argv) { | |||
|   cout << "===================================================" << endl; | ||||
| 
 | ||||
|   // --------------- WRITE OUTPUT TO BAL FILE  ----------------------------------------
 | ||||
|   if(useSmartProjectionFactor){ | ||||
|     smartCreator.computePoints(result); | ||||
|   } | ||||
| 
 | ||||
|   cout << "- writing results to (BAL) file... " << endl; | ||||
|   std::size_t stringCut1 = datasetFile.rfind("/"); | ||||
|   std::size_t stringCut2 = datasetFile.rfind(".txt"); | ||||
|  |  | |||
|  | @ -43,6 +43,22 @@ namespace gtsam { | |||
|           linearizationThreshold_(linThreshold), body_P_sensor_(body_P_sensor), | ||||
|           totalNumMeasurements(0), numLandmarks(0) {}; | ||||
| 
 | ||||
|     void computePoints(Values& values) { | ||||
| 
 | ||||
|       typename SmartFactorMap::iterator fit; | ||||
|       // Check if landmark exists in mapping
 | ||||
|       for(fit = smartFactors.begin(); fit != smartFactors.end(); fit++) { | ||||
|         Key pointKey = (*fit).first; | ||||
|         Point3 currentPoint; | ||||
|         if((*fit).second->point() && !(*fit).second->isDegenerate()){ | ||||
|           currentPoint = *((*fit).second->point()); | ||||
|         }else{ | ||||
|           currentPoint = Point3(); // if we cannot the smartFactor is degenerate
 | ||||
|         } | ||||
|         values.insert(pointKey, currentPoint); | ||||
|       } | ||||
|     } | ||||
| 
 | ||||
|     void add(Key landmarkKey, | ||||
|         Key poseKey, Point2 measurement, NonlinearFactorGraph &graph) { | ||||
| 
 | ||||
|  |  | |||
|  | @ -581,6 +581,11 @@ namespace gtsam { | |||
|       return K_all_; | ||||
|     } | ||||
| 
 | ||||
|     /** return the calibration object */ | ||||
|     inline bool isDegenerate() const { | ||||
|       return (state_->cheiralityException || state_->degenerate); | ||||
|     } | ||||
| 
 | ||||
|     /** return verbosity */ | ||||
|     inline bool verboseCheirality() const { return verboseCheirality_; } | ||||
| 
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue