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

Using an "Empty" constant in VariableSlots instead of calling numeric_limits::max() each time

release/4.3a0
Richard Roberts 2013-12-17 11:36:26 -05:00
parent 09309e2869
commit 2553e39746
3 changed files with 7 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/inference/VariableSlots.cpp
View File

@ -24,6 +24,8 @@ using namespace std;
namespace gtsam { namespace gtsam {
const size_t VariableSlots::Empty = numeric_limits<size_t>::max();
/** print */ /** print */
void VariableSlots::print(const std::string& str) const { void VariableSlots::print(const std::string& str) const {
if(this->empty()) if(this->empty())
@ -37,7 +39,7 @@ void VariableSlots::print(const std::string& str) const {
for(size_t i=0; i<this->begin()->second.size(); ++i) { for(size_t i=0; i<this->begin()->second.size(); ++i) {
cout << " \t"; cout << " \t";
BOOST_FOREACH(const value_type& slot, *this) { BOOST_FOREACH(const value_type& slot, *this) {
if(slot.second[i] == numeric_limits<size_t>::max()) if(slot.second[i] == Empty)
cout << "x" << "\t"; cout << "x" << "\t";
else else
cout << slot.second[i] << "\t"; cout << slot.second[i] << "\t";

3
gtsam/inference/VariableSlots.h
View File

@ -55,6 +55,7 @@ class VariableSlots : public FastMap<Key, FastVector<size_t> > {
public: public:
typedef FastMap<Key, FastVector<size_t> > Base; typedef FastMap<Key, FastVector<size_t> > Base;
static const size_t Empty;
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{
@ -108,7 +109,7 @@ VariableSlots::VariableSlots(const FG& factorGraph)
iterator thisVarSlots; bool inserted; iterator thisVarSlots; bool inserted;
boost::tie(thisVarSlots, inserted) = this->insert(std::make_pair(involvedVariable, FastVector<size_t>())); boost::tie(thisVarSlots, inserted) = this->insert(std::make_pair(involvedVariable, FastVector<size_t>()));
if(inserted) if(inserted)
thisVarSlots->second.resize(factorGraph.size(), std::numeric_limits<size_t>::max()); thisVarSlots->second.resize(factorGraph.size(), Empty);
thisVarSlots->second[jointFactorPos] = factorVarSlot; thisVarSlots->second[jointFactorPos] = factorVarSlot;
if(debug) std::cout << " var " << involvedVariable << " rowblock " << jointFactorPos << " comes from factor's slot " << factorVarSlot << std::endl; if(debug) std::cout << " var " << involvedVariable << " rowblock " << jointFactorPos << " comes from factor's slot " << factorVarSlot << std::endl;
++ factorVarSlot; ++ factorVarSlot;

4
gtsam/linear/JacobianFactor.cpp
View File

@ -158,7 +158,7 @@ namespace gtsam {
for(size_t sourceFactorI = 0; sourceFactorI < slots->second.size(); ++sourceFactorI) for(size_t sourceFactorI = 0; sourceFactorI < slots->second.size(); ++sourceFactorI)
{ {
const size_t sourceVarpos = slots->second[sourceFactorI]; const size_t sourceVarpos = slots->second[sourceFactorI];
if(sourceVarpos < numeric_limits<size_t>::max()) { if(sourceVarpos != VariableSlots::Empty) {
const JacobianFactor& sourceFactor = *factors[sourceFactorI]; const JacobianFactor& sourceFactor = *factors[sourceFactorI];
if(sourceFactor.cols() > 1) { if(sourceFactor.cols() > 1) {
foundVariable = true; foundVariable = true;
@ -303,7 +303,7 @@ namespace gtsam {
if(sourceRows > 0) { if(sourceRows > 0) {
JacobianFactor::ABlock::RowsBlockXpr destBlock(destSlot.middleRows(nextRow, sourceRows)); JacobianFactor::ABlock::RowsBlockXpr destBlock(destSlot.middleRows(nextRow, sourceRows));
// Copy if exists in source factor, otherwise set zero // Copy if exists in source factor, otherwise set zero
if(sourceSlot != numeric_limits<size_t>::max()) if(sourceSlot != VariableSlots::Empty)
destBlock = jacobians[factorI]->getA(jacobians[factorI]->begin()+sourceSlot); destBlock = jacobians[factorI]->getA(jacobians[factorI]->begin()+sourceSlot);
else else
destBlock.setZero(); destBlock.setZero();