Skip to content
Snippets Groups Projects
Commit dc6bd56b authored by Jeremie Deray's avatar Jeremie Deray Committed by Jeremie Deray
Browse files

emplace_back rather than push_back(std::make_pair in CeresManager

parent 51e0b5e3
No related branches found
No related tags found
1 merge request!126Miscellaneous
...@@ -105,8 +105,8 @@ void CeresManager::computeCovariances(CovarianceBlocksToBeComputed _blocks) ...@@ -105,8 +105,8 @@ void CeresManager::computeCovariances(CovarianceBlocksToBeComputed _blocks)
{ {
for (unsigned int j = i; j < all_state_blocks.size(); j++) for (unsigned int j = i; j < all_state_blocks.size(); j++)
{ {
state_block_pairs.push_back(std::make_pair(all_state_blocks[i],all_state_blocks[j])); state_block_pairs.emplace_back(all_state_blocks[i],all_state_blocks[j]);
double_pairs.push_back(std::make_pair(all_state_blocks[i]->getPtr(),all_state_blocks[j]->getPtr())); double_pairs.emplace_back(all_state_blocks[i]->getPtr(),all_state_blocks[j]->getPtr());
} }
} }
break; break;
...@@ -128,8 +128,8 @@ void CeresManager::computeCovariances(CovarianceBlocksToBeComputed _blocks) ...@@ -128,8 +128,8 @@ void CeresManager::computeCovariances(CovarianceBlocksToBeComputed _blocks)
{ {
if (sb) if (sb)
{ {
state_block_pairs.push_back(std::make_pair(sb, sb2)); state_block_pairs.emplace_back(sb, sb2);
double_pairs.push_back(std::make_pair(sb->getPtr(), sb2->getPtr())); double_pairs.emplace_back(sb->getPtr(), sb2->getPtr());
if (sb == sb2) break; if (sb == sb2) break;
} }
} }
...@@ -151,8 +151,8 @@ void CeresManager::computeCovariances(CovarianceBlocksToBeComputed _blocks) ...@@ -151,8 +151,8 @@ void CeresManager::computeCovariances(CovarianceBlocksToBeComputed _blocks)
// all_state_blocks.push_back(sb); // all_state_blocks.push_back(sb);
for(auto sb2 : l_ptr->getUsedStateBlockVec()) for(auto sb2 : l_ptr->getUsedStateBlockVec())
{ {
state_block_pairs.push_back(std::make_pair(sb, sb2)); state_block_pairs.emplace_back(sb, sb2);
double_pairs.push_back(std::make_pair(sb->getPtr(), sb2->getPtr())); double_pairs.emplace_back(sb->getPtr(), sb2->getPtr());
if (sb == sb2) break; if (sb == sb2) break;
} }
} }
...@@ -163,13 +163,13 @@ void CeresManager::computeCovariances(CovarianceBlocksToBeComputed _blocks) ...@@ -163,13 +163,13 @@ void CeresManager::computeCovariances(CovarianceBlocksToBeComputed _blocks)
// // loop all marginals (PO marginals) // // loop all marginals (PO marginals)
// for (unsigned int i = 0; 2*i+1 < all_state_blocks.size(); i++) // for (unsigned int i = 0; 2*i+1 < all_state_blocks.size(); i++)
// { // {
// state_block_pairs.push_back(std::make_pair(all_state_blocks[2*i],all_state_blocks[2*i])); // state_block_pairs.emplace_back(all_state_blocks[2*i],all_state_blocks[2*i]);
// state_block_pairs.push_back(std::make_pair(all_state_blocks[2*i],all_state_blocks[2*i+1])); // state_block_pairs.emplace_back(all_state_blocks[2*i],all_state_blocks[2*i+1]);
// state_block_pairs.push_back(std::make_pair(all_state_blocks[2*i+1],all_state_blocks[2*i+1])); // state_block_pairs.emplace_back(all_state_blocks[2*i+1],all_state_blocks[2*i+1]);
// //
// double_pairs.push_back(std::make_pair(all_state_blocks[2*i]->getPtr(),all_state_blocks[2*i]->getPtr())); // double_pairs.emplace_back(all_state_blocks[2*i]->getPtr(),all_state_blocks[2*i]->getPtr());
// double_pairs.push_back(std::make_pair(all_state_blocks[2*i]->getPtr(),all_state_blocks[2*i+1]->getPtr())); // double_pairs.emplace_back(all_state_blocks[2*i]->getPtr(),all_state_blocks[2*i+1]->getPtr());
// double_pairs.push_back(std::make_pair(all_state_blocks[2*i+1]->getPtr(),all_state_blocks[2*i+1]->getPtr())); // double_pairs.emplace_back(all_state_blocks[2*i+1]->getPtr(),all_state_blocks[2*i+1]->getPtr());
// } // }
break; break;
} }
...@@ -178,13 +178,13 @@ void CeresManager::computeCovariances(CovarianceBlocksToBeComputed _blocks) ...@@ -178,13 +178,13 @@ void CeresManager::computeCovariances(CovarianceBlocksToBeComputed _blocks)
//robot-robot //robot-robot
auto last_key_frame = wolf_problem_->getLastKeyFramePtr(); auto last_key_frame = wolf_problem_->getLastKeyFramePtr();
state_block_pairs.push_back(std::make_pair(last_key_frame->getPPtr(), last_key_frame->getPPtr())); state_block_pairs.emplace_back(last_key_frame->getPPtr(), last_key_frame->getPPtr());
state_block_pairs.push_back(std::make_pair(last_key_frame->getPPtr(), last_key_frame->getOPtr())); state_block_pairs.emplace_back(last_key_frame->getPPtr(), last_key_frame->getOPtr());
state_block_pairs.push_back(std::make_pair(last_key_frame->getOPtr(), last_key_frame->getOPtr())); state_block_pairs.emplace_back(last_key_frame->getOPtr(), last_key_frame->getOPtr());
double_pairs.push_back(std::make_pair(last_key_frame->getPPtr()->getPtr(), last_key_frame->getPPtr()->getPtr())); double_pairs.emplace_back(last_key_frame->getPPtr()->getPtr(), last_key_frame->getPPtr()->getPtr());
double_pairs.push_back(std::make_pair(last_key_frame->getPPtr()->getPtr(), last_key_frame->getOPtr()->getPtr())); double_pairs.emplace_back(last_key_frame->getPPtr()->getPtr(), last_key_frame->getOPtr()->getPtr());
double_pairs.push_back(std::make_pair(last_key_frame->getOPtr()->getPtr(), last_key_frame->getOPtr()->getPtr())); double_pairs.emplace_back(last_key_frame->getOPtr()->getPtr(), last_key_frame->getOPtr()->getPtr());
// landmarks // landmarks
std::vector<StateBlockPtr> landmark_state_blocks; std::vector<StateBlockPtr> landmark_state_blocks;
...@@ -196,16 +196,16 @@ void CeresManager::computeCovariances(CovarianceBlocksToBeComputed _blocks) ...@@ -196,16 +196,16 @@ void CeresManager::computeCovariances(CovarianceBlocksToBeComputed _blocks)
for (auto state_it = landmark_state_blocks.begin(); state_it != landmark_state_blocks.end(); state_it++) for (auto state_it = landmark_state_blocks.begin(); state_it != landmark_state_blocks.end(); state_it++)
{ {
// robot - landmark // robot - landmark
state_block_pairs.push_back(std::make_pair(last_key_frame->getPPtr(), *state_it)); state_block_pairs.emplace_back(last_key_frame->getPPtr(), *state_it);
state_block_pairs.push_back(std::make_pair(last_key_frame->getOPtr(), *state_it)); state_block_pairs.emplace_back(last_key_frame->getOPtr(), *state_it);
double_pairs.push_back(std::make_pair(last_key_frame->getPPtr()->getPtr(), (*state_it)->getPtr())); double_pairs.emplace_back(last_key_frame->getPPtr()->getPtr(), (*state_it)->getPtr());
double_pairs.push_back(std::make_pair(last_key_frame->getOPtr()->getPtr(), (*state_it)->getPtr())); double_pairs.emplace_back(last_key_frame->getOPtr()->getPtr(), (*state_it)->getPtr());
// landmark marginal // landmark marginal
for (auto next_state_it = state_it; next_state_it != landmark_state_blocks.end(); next_state_it++) for (auto next_state_it = state_it; next_state_it != landmark_state_blocks.end(); next_state_it++)
{ {
state_block_pairs.push_back(std::make_pair(*state_it, *next_state_it)); state_block_pairs.emplace_back(*state_it, *next_state_it);
double_pairs.push_back(std::make_pair((*state_it)->getPtr(), (*next_state_it)->getPtr())); double_pairs.emplace_back((*state_it)->getPtr(), (*next_state_it)->getPtr());
} }
} }
} }
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment