12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fixed small c++ issues (const, reference) and formatted

release/4.3a0
dellaert 2015-06-20 11:49:13 -07:00
parent e6a563d900
commit 51df837f74
2 changed files with 45 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
gtsam/slam/SmartProjectionFactor.h
View File

@ -64,25 +64,30 @@ public:
/// @} /// @}
// Constructor // Constructor
SmartProjectionParams(const LinearizationMode linMode = HESSIAN, SmartProjectionParams(LinearizationMode linMode = HESSIAN,
const DegeneracyMode degMode = IGNORE_DEGENERACY, const double rankTol = 1, DegeneracyMode degMode = IGNORE_DEGENERACY, double rankTol = 1,
const bool enableEPI = false, const double landmarkDistanceThreshold = 1e10, bool enableEPI = false, double landmarkDistanceThreshold = 1e10,
const double dynamicOutlierRejectionThreshold = -1): double dynamicOutlierRejectionThreshold = -1) :
linearizationMode(linMode), degeneracyMode(degMode), linearizationMode(linMode), degeneracyMode(degMode), triangulationParameters(
triangulationParameters(rankTol, enableEPI, rankTol, enableEPI, landmarkDistanceThreshold,
landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), dynamicOutlierRejectionThreshold), retriangulationThreshold(1e-5), throwCheirality(
retriangulationThreshold(1e-5), false), verboseCheirality(false) {
throwCheirality(false), verboseCheirality(false) {} }
virtual ~SmartProjectionParams() {} virtual ~SmartProjectionParams() {
}
void print(const std::string& str) const { void print(const std::string& str) const {
std::cout << " linearizationMode: " << linearizationMode << "\n"; std::cout << " linearizationMode: " << linearizationMode << "\n";
std::cout << " degeneracyMode: " << degeneracyMode << "\n"; std::cout << " degeneracyMode: " << degeneracyMode << "\n";
std::cout << " rankTolerance: " << triangulationParameters.rankTolerance << "\n"; std::cout << " rankTolerance: "
std::cout << " enableEPI: " << triangulationParameters.enableEPI << "\n"; << triangulationParameters.rankTolerance << "\n";
std::cout << " landmarkDistanceThreshold: " << triangulationParameters.landmarkDistanceThreshold << "\n"; std::cout << " enableEPI: "
std::cout << " OutlierRejectionThreshold: " << triangulationParameters.dynamicOutlierRejectionThreshold << "\n"; << triangulationParameters.enableEPI << "\n";
std::cout << " landmarkDistanceThreshold: "
<< triangulationParameters.landmarkDistanceThreshold << "\n";
std::cout << " OutlierRejectionThreshold: "
<< triangulationParameters.dynamicOutlierRejectionThreshold << "\n";
std::cout.flush(); std::cout.flush();
} }
@ -114,10 +119,13 @@ public:
triangulationParameters.enableEPI = enableEPI; triangulationParameters.enableEPI = enableEPI;
} }
inline void setLandmarkDistanceThreshold(bool landmarkDistanceThreshold) { inline void setLandmarkDistanceThreshold(bool landmarkDistanceThreshold) {
triangulationParameters.landmarkDistanceThreshold = landmarkDistanceThreshold; triangulationParameters.landmarkDistanceThreshold =
landmarkDistanceThreshold;
} }
inline void setDynamicOutlierRejectionThreshold(bool dynOutRejectionThreshold) { inline void setDynamicOutlierRejectionThreshold(
triangulationParameters.dynamicOutlierRejectionThreshold = dynOutRejectionThreshold; bool dynOutRejectionThreshold) {
triangulationParameters.dynamicOutlierRejectionThreshold =
dynOutRejectionThreshold;
} }
}; };
@ -160,11 +168,12 @@ public:
* @param body_P_sensor pose of the camera in the body frame * @param body_P_sensor pose of the camera in the body frame
* @param params internal parameters of the smart factors * @param params internal parameters of the smart factors
*/ */
SmartProjectionFactor(const boost::optional<Pose3> body_P_sensor = boost::none, SmartProjectionFactor(
const SmartProjectionParams params = SmartProjectionParams()) : const boost::optional<Pose3> body_P_sensor = boost::none,
Base(body_P_sensor), const SmartProjectionParams& params = SmartProjectionParams()) :
params_(params), // Base(body_P_sensor), params_(params), //
result_(TriangulationResult::Degenerate()) {} result_(TriangulationResult::Degenerate()) {
}
/** Virtual destructor */ /** Virtual destructor */
virtual ~SmartProjectionFactor() { virtual ~SmartProjectionFactor() {
@ -178,8 +187,10 @@ public:
void print(const std::string& s = "", const KeyFormatter& keyFormatter = void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const { DefaultKeyFormatter) const {
std::cout << s << "SmartProjectionFactor\n"; std::cout << s << "SmartProjectionFactor\n";
std::cout << "linearizationMode:\n" << params_.linearizationMode << std::endl; std::cout << "linearizationMode:\n" << params_.linearizationMode
std::cout << "triangulationParameters:\n" << params_.triangulationParameters << std::endl; << std::endl;
std::cout << "triangulationParameters:\n" << params_.triangulationParameters
<< std::endl;
std::cout << "result:\n" << result_ << std::endl; std::cout << "result:\n" << result_ << std::endl;
Base::print("", keyFormatter); Base::print("", keyFormatter);
} }
@ -236,7 +247,8 @@ public:
bool retriangulate = decideIfTriangulate(cameras); bool retriangulate = decideIfTriangulate(cameras);
if (retriangulate) if (retriangulate)
result_ = gtsam::triangulateSafe(cameras, this->measured_, params_.triangulationParameters); result_ = gtsam::triangulateSafe(cameras, this->measured_,
params_.triangulationParameters);
return result_; return result_;
} }

13
gtsam/slam/SmartProjectionPoseFactor.h
View File

@ -63,11 +63,13 @@ public:
*/ */
SmartProjectionPoseFactor(const boost::shared_ptr<CALIBRATION> K, SmartProjectionPoseFactor(const boost::shared_ptr<CALIBRATION> K,
const boost::optional<Pose3> body_P_sensor = boost::none, const boost::optional<Pose3> body_P_sensor = boost::none,
const SmartProjectionParams params = SmartProjectionParams()) : const SmartProjectionParams& params = SmartProjectionParams()) :
Base(body_P_sensor, params), K_(K) {} Base(body_P_sensor, params), K_(K) {
}
/** Virtual destructor */ /** Virtual destructor */
virtual ~SmartProjectionPoseFactor() {} virtual ~SmartProjectionPoseFactor() {
}
/** /**
* print * print
@ -112,7 +114,7 @@ public:
typename Base::Cameras cameras; typename Base::Cameras cameras;
BOOST_FOREACH(const Key& k, this->keys_) { BOOST_FOREACH(const Key& k, this->keys_) {
Pose3 pose = values.at<Pose3>(k); Pose3 pose = values.at<Pose3>(k);
if(Base::body_P_sensor_) if (Base::body_P_sensor_)
pose = pose.compose(*(Base::body_P_sensor_)); pose = pose.compose(*(Base::body_P_sensor_));
Camera camera(pose, K_); Camera camera(pose, K_);
@ -131,7 +133,8 @@ private:
ar & BOOST_SERIALIZATION_NVP(K_); ar & BOOST_SERIALIZATION_NVP(K_);
} }
}; // end of class declaration };
// end of class declaration
/// traits /// traits
template<class CALIBRATION> template<class CALIBRATION>