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

Give access to bearing/range

release/4.3a0
dellaert 2018-11-06 13:28:47 -05:00
parent 4113d99e20
commit 79d63010dd
2 changed files with 28 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
examples/SolverComparer.cpp
View File

@ -251,10 +251,10 @@ void runIncremental()
Key firstPose; Key firstPose;
while(nextMeasurement < datasetMeasurements.size()) while(nextMeasurement < datasetMeasurements.size())
{ {
if(BetweenFactor<Pose>::shared_ptr measurement = if(BetweenFactor<Pose>::shared_ptr factor =
boost::dynamic_pointer_cast<BetweenFactor<Pose> >(datasetMeasurements[nextMeasurement])) boost::dynamic_pointer_cast<BetweenFactor<Pose> >(datasetMeasurements[nextMeasurement]))
{ {
Key key1 = measurement->key1(), key2 = measurement->key2(); Key key1 = factor->key1(), key2 = factor->key2();
if(((int)key1 >= firstStep && key1 < key2) || ((int)key2 >= firstStep && key2 < key1)) { if(((int)key1 >= firstStep && key1 < key2) || ((int)key2 >= firstStep && key2 < key1)) {
// We found an odometry starting at firstStep // We found an odometry starting at firstStep
firstPose = std::min(key1, key2); firstPose = std::min(key1, key2);
@ -302,52 +302,53 @@ void runIncremental()
NonlinearFactor::shared_ptr measurementf = datasetMeasurements[nextMeasurement]; NonlinearFactor::shared_ptr measurementf = datasetMeasurements[nextMeasurement];
if(BetweenFactor<Pose>::shared_ptr measurement = if(BetweenFactor<Pose>::shared_ptr factor =
boost::dynamic_pointer_cast<BetweenFactor<Pose> >(measurementf)) boost::dynamic_pointer_cast<BetweenFactor<Pose> >(measurementf))
{ {
// Stop collecting measurements that are for future steps // Stop collecting measurements that are for future steps
if(measurement->key1() > step || measurement->key2() > step) if(factor->key1() > step || factor->key2() > step)
break; break;
// Require that one of the nodes is the current one // Require that one of the nodes is the current one
if(measurement->key1() != step && measurement->key2() != step) if(factor->key1() != step && factor->key2() != step)
throw runtime_error("Problem in data file, out-of-sequence measurements"); throw runtime_error("Problem in data file, out-of-sequence measurements");
// Add a new factor // Add a new factor
newFactors.push_back(measurement); newFactors.push_back(factor);
const auto& measured = factor->measured();
// Initialize the new variable // Initialize the new variable
if(measurement->key1() > measurement->key2()) { if(factor->key1() > factor->key2()) {
if(!newVariables.exists(measurement->key1())) { // Only need to check newVariables since loop closures come after odometry if(!newVariables.exists(factor->key1())) { // Only need to check newVariables since loop closures come after odometry
if(step == 1) if(step == 1)
newVariables.insert(measurement->key1(), measurement->measured().inverse()); newVariables.insert(factor->key1(), measured.inverse());
else { else {
Pose prevPose = isam2.calculateEstimate<Pose>(measurement->key2()); Pose prevPose = isam2.calculateEstimate<Pose>(factor->key2());
newVariables.insert(measurement->key1(), prevPose * measurement->measured().inverse()); newVariables.insert(factor->key1(), prevPose * measured.inverse());
} }
} }
} else { } else {
if(!newVariables.exists(measurement->key2())) { // Only need to check newVariables since loop closures come after odometry if(!newVariables.exists(factor->key2())) { // Only need to check newVariables since loop closures come after odometry
if(step == 1) if(step == 1)
newVariables.insert(measurement->key2(), measurement->measured()); newVariables.insert(factor->key2(), measured);
else { else {
Pose prevPose = isam2.calculateEstimate<Pose>(measurement->key1()); Pose prevPose = isam2.calculateEstimate<Pose>(factor->key1());
newVariables.insert(measurement->key2(), prevPose * measurement->measured()); newVariables.insert(factor->key2(), prevPose * measured);
} }
} }
} }
} }
else if(BearingRangeFactor<Pose, Point2>::shared_ptr measurement = else if(BearingRangeFactor<Pose, Point2>::shared_ptr factor =
boost::dynamic_pointer_cast<BearingRangeFactor<Pose, Point2> >(measurementf)) boost::dynamic_pointer_cast<BearingRangeFactor<Pose, Point2> >(measurementf))
{ {
Key poseKey = measurement->keys()[0], lmKey = measurement->keys()[1]; Key poseKey = factor->keys()[0], lmKey = factor->keys()[1];
// Stop collecting measurements that are for future steps // Stop collecting measurements that are for future steps
if(poseKey > step) if(poseKey > step)
throw runtime_error("Problem in data file, out-of-sequence measurements"); throw runtime_error("Problem in data file, out-of-sequence measurements");
// Add new factor // Add new factor
newFactors.push_back(measurement); newFactors.push_back(factor);
// Initialize new landmark // Initialize new landmark
if(!isam2.getLinearizationPoint().exists(lmKey)) if(!isam2.getLinearizationPoint().exists(lmKey))
@ -357,8 +358,9 @@ void runIncremental()
pose = isam2.calculateEstimate<Pose>(poseKey); pose = isam2.calculateEstimate<Pose>(poseKey);
else else
pose = newVariables.at<Pose>(poseKey); pose = newVariables.at<Pose>(poseKey);
Rot2 measuredBearing = measurement->measured().first; const auto& measured = factor->measured();
double measuredRange = measurement->measured().second; Rot2 measuredBearing = measured.bearing();
double measuredRange = measured.range();
newVariables.insert(lmKey, newVariables.insert(lmKey,
pose.transform_from(measuredBearing.rotate(Point2(measuredRange, 0.0)))); pose.transform_from(measuredBearing.rotate(Point2(measuredRange, 0.0))));
} }

6
gtsam/geometry/BearingRange.h
View File

@ -67,6 +67,12 @@ public:
/// @name Standard Interface /// @name Standard Interface
/// @{ /// @{
/// Return bearing measurement
const B& bearing() const { return bearing_; }
/// Return range measurement
const R& range() const { return range_; }
/// Prediction function that stacks measurements /// Prediction function that stacks measurements
static BearingRange Measure( static BearingRange Measure(
const A1& a1, const A2& a2, const A1& a1, const A2& a2,