Permalink
Browse files

clang-format

  • Loading branch information...
jlblancoc committed Nov 19, 2018
1 parent 7c909ca commit d01bdcb7bdfd532786d2d58b9ee094194fb96472
@@ -596,12 +596,12 @@ void do_grid_align()
lmap2 = CLandmarksMap::Create();

gridfeatextract.extractFeatures(
*grid1, *lmap1, N1,
gridAlign.options.feature_descriptor,
*grid1, *lmap1, N1,
gridAlign.options.feature_descriptor,
gridAlign.options.feature_detector_options);
gridfeatextract.extractFeatures(
*grid2, *lmap2, N2,
gridAlign.options.feature_descriptor,
*grid2, *lmap2, N2,
gridAlign.options.feature_descriptor,
gridAlign.options.feature_detector_options);
}

@@ -33,8 +33,8 @@ DECLARE_OP_FUNCTION(op_list_poses)
public:
CRawlogProcessor_ListPoses(
CFileGZInputStream& in_rawlog, TCLAP::CmdLine& cmdline,
bool Verbose)
: CRawlogProcessorOnEachObservation(in_rawlog, cmdline, Verbose)
bool Verbose)
: CRawlogProcessorOnEachObservation(in_rawlog, cmdline, Verbose)
{
getArgValue<std::string>(cmdline, "text-file-output", m_out_file);
VERBOSE_COUT << "Writing list to: " << m_out_file << endl;
@@ -179,7 +179,7 @@ void CKalmanFilterCapable<
// can decide if their covariances (more costly) must be computed as well:
m_all_predictions.resize(N_map);
OnObservationModel(
mrpt::math::sequenceStdVec<size_t, 1>(0, N_map), m_all_predictions);
mrpt::math::sequenceStdVec<size_t, 1>(0, N_map), m_all_predictions);

const double tim_pred_obs = m_timLogger.leave("KF:3.predict all obs");

@@ -223,7 +223,7 @@ void CKalmanFilterCapable<
size_t N_pred =
FEAT_SIZE == 0
? 1 /* In non-SLAM problems, there'll be only 1 fixed observation */
: m_predictLMidxs.size();
: m_predictLMidxs.size();

std::vector<int>
data_association; // -1: New map feature.>=0: Indexes in the
@@ -383,7 +383,7 @@ void CKalmanFilterCapable<
const size_t lm_idx_j = m_predictLMidxs[j];
// Sij block:
Eigen::Block<typename KFMatrix::Base, OBS_SIZE, OBS_SIZE>
Sij(m_S, OBS_SIZE * i, OBS_SIZE * j);
Sij(m_S, OBS_SIZE * i, OBS_SIZE * j);

const Eigen::Block<
const typename KFMatrix::Base, VEH_SIZE, FEAT_SIZE>
@@ -395,21 +395,21 @@ void CKalmanFilterCapable<
VEH_SIZE + lm_idx_j * FEAT_SIZE);

Sij = m_Hxs[i] * Px * m_Hxs[j].transpose() +
m_Hys[i] * Pxyi_t * m_Hxs[j].transpose() +
m_Hxs[i] * Pxyj * m_Hys[j].transpose() +
m_Hys[i] * Pyiyj * m_Hys[j].transpose();
m_Hys[i] * Pxyi_t * m_Hxs[j].transpose() +
m_Hxs[i] * Pxyj * m_Hys[j].transpose() +
m_Hys[i] * Pyiyj * m_Hys[j].transpose();

// Copy transposed to the symmetric lower-triangular part:
if (i != j)
Eigen::Block<
typename KFMatrix::Base, OBS_SIZE, OBS_SIZE>(
m_S, OBS_SIZE * j, OBS_SIZE * i) = Sij.transpose();
m_S, OBS_SIZE * j, OBS_SIZE * i) = Sij.transpose();
}

// Sum the "R" term to the diagonal blocks:
const size_t obs_idx_off = i * OBS_SIZE;
Eigen::Block<typename KFMatrix::Base, OBS_SIZE, OBS_SIZE>(
m_S, obs_idx_off, obs_idx_off) += R;
m_S, obs_idx_off, obs_idx_off) += R;
}
}
else
@@ -428,11 +428,11 @@ void CKalmanFilterCapable<

// Get observations and do data-association:
OnGetObservationsAndDataAssociation(
m_Z, data_association, // Out
m_all_predictions, m_S, m_predictLMidxs, R // In
m_Z, data_association, // Out
m_all_predictions, m_S, m_predictLMidxs, R // In
);
ASSERTDEB_(
data_association.size() == m_Z.size() ||
data_association.size() == m_Z.size() ||
(data_association.empty() && FEAT_SIZE == 0));

// Check if an observation hasn't been predicted in
@@ -449,7 +449,7 @@ void CKalmanFilterCapable<
const auto assoc_idx_in_map = static_cast<size_t>(i);
const size_t assoc_idx_in_pred =
mrpt::containers::find_in_vector(
assoc_idx_in_map, m_predictLMidxs);
assoc_idx_in_map, m_predictLMidxs);
if (assoc_idx_in_pred == std::string::npos)
missing_predictions_to_add.push_back(assoc_idx_in_map);
}
@@ -538,7 +538,7 @@ void CKalmanFilterCapable<
static_cast<size_t>(data_association[i]);
const size_t assoc_idx_in_pred =
mrpt::containers::find_in_vector(
assoc_idx_in_map, m_predictLMidxs);
assoc_idx_in_map, m_predictLMidxs);
ASSERTMSG_(
assoc_idx_in_pred != string::npos,
"OnPreComputingPredictions() didn't "
@@ -560,13 +560,13 @@ void CKalmanFilterCapable<
typename KFMatrix::Base, OBS_SIZE,
VEH_SIZE>(
m_dh_dx_full_obs, S_idxs.size(), 0) =
m_Hxs[assoc_idx_in_pred];
m_Hxs[assoc_idx_in_pred];
Eigen::Block<
typename KFMatrix::Base, OBS_SIZE,
FEAT_SIZE>(
m_dh_dx_full_obs, S_idxs.size(),
VEH_SIZE + assoc_idx_in_map * FEAT_SIZE) =
m_Hys[assoc_idx_in_pred];
m_Hys[assoc_idx_in_pred];

// S_idxs.size() is used as counter for
// "m_dh_dx_full_obs".
@@ -578,8 +578,8 @@ void CKalmanFilterCapable<
KFArray_OBS ytilde_i = m_Z[i];
OnSubstractObservationVectors(
ytilde_i,
m_all_predictions
[m_predictLMidxs[assoc_idx_in_pred]]);
m_all_predictions
[m_predictLMidxs[assoc_idx_in_pred]]);
for (size_t k = 0; k < OBS_SIZE; k++)
ytilde[ytilde_idx++] = ytilde_i[k];
}
@@ -590,16 +590,16 @@ void CKalmanFilterCapable<
else
{ // Non-SLAM problems:
ASSERT_(
m_Z.size() == 1 &&
m_all_predictions.size() == 1);
m_Z.size() == 1 &&
m_all_predictions.size() == 1);
ASSERT_(
m_dh_dx_full_obs.rows() == OBS_SIZE &&
m_dh_dx_full_obs.cols() == VEH_SIZE);
ASSERT_(m_Hxs.size() == 1);
m_dh_dx_full_obs = m_Hxs[0]; // Was: dh_dx_full
KFArray_OBS ytilde_i = m_Z[0];
OnSubstractObservationVectors(
ytilde_i, m_all_predictions[0]);
ytilde_i, m_all_predictions[0]);
for (size_t k = 0; k < OBS_SIZE; k++)
ytilde[ytilde_idx++] = ytilde_i[k];
// Extract the subset that is involved in this
@@ -698,7 +698,7 @@ void CKalmanFilterCapable<
case kfEKFAlaDavison:
{
// For each observed landmark/whole system state:
for (size_t obsIdx = 0; obsIdx < m_Z.size(); obsIdx++)
for (size_t obsIdx = 0; obsIdx < m_Z.size(); obsIdx++)
{
// Known & mapped landmark?
bool doit;
@@ -743,7 +743,7 @@ void CKalmanFilterCapable<

const size_t i_idx_in_preds =
mrpt::containers::find_in_vector(
idxInTheFilter, m_predictLMidxs);
idxInTheFilter, m_predictLMidxs);
ASSERTMSG_(
i_idx_in_preds != string::npos,
"OnPreComputingPredictions() didn't recommend the "
@@ -751,11 +751,11 @@ void CLoopCloserERD<GRAPH_T>::generateHypotsPool(
#endif
// map should have same size
ASSERTDEBMSG_(
nodes_count == p.size(),
nodes_count == p.size(),
format(
"Size mismatch between nodeIDs in group [%lu]"
" and corresponding properties map [%lu]",
nodes_count, p.size()));
nodes_count, p.size()));
}
}

@@ -314,7 +314,7 @@ void CEdgeCounter::updateTextMessages() const
std::stringstream ss;
ss << " " << name << ": " << edges_num << std::endl;
m_win_manager->addTextMessage(
5, -offset_y, ss.str(), mrpt::img::TColorf(1.0, 1.0, 1.0),
5, -offset_y, ss.str(), mrpt::img::TColorf(1.0, 1.0, 1.0),
/* unique_index = */ text_index);
}

@@ -325,7 +325,7 @@ void CEdgeCounter::updateTextMessages() const
ss << " "
<< "Loop closures: " << m_num_loop_closures << std::endl;
m_win_manager->addTextMessage(
5, -m_offset_y_loop_closures, ss.str(),
5, -m_offset_y_loop_closures, ss.str(),
mrpt::img::TColorf(1.0, 1.0, 1.0),
/* unique_index = */ m_text_index_loop_closures);
}
@@ -61,7 +61,7 @@ void CHMTSLAM::thread_LSLAM()
// Start thread:
// -------------------------
MRPT_LOG_DEBUG_STREAM(
"[thread_LSLAM] Thread started ID=" << std::this_thread::get_id());
"[thread_LSLAM] Thread started ID=" << std::this_thread::get_id());

// --------------------------------------------
// The main loop
@@ -406,7 +406,7 @@ void CHMTSLAM::LSLAM_process_message_from_AA(const TMessageLSLAMfromAA& myMsg)
{
vector<TPoseIDList>::const_iterator it;
for (i = 0, it = myMsg.partitions.begin(); it != myMsg.partitions.end();
++it, i++)
++it, i++)
for (unsigned long itPose : *it)
{
auto itP = LMH->m_nodeIDmemberships.find(itPose);
@@ -515,9 +515,9 @@ void CHMTSLAM::LSLAM_process_message_from_AA(const TMessageLSLAMfromAA& myMsg)
logFmt(mrpt::system::LVL_INFO, "[LSLAM_proc_msg_AA] partIdx2Areas:\n");
for (unsigned idx = 0; idx < partIdx2Areas.size(); idx++)
MRPT_LOG_INFO_STREAM(
"Partition "
<< idx << " -> AREA_ID " << partIdx2Areas[idx] << " ('"
<< m_map.getNodeByID(partIdx2Areas[idx])->m_label << "')\n");
"Partition "
<< idx << " -> AREA_ID " << partIdx2Areas[idx] << " ('"
<< m_map.getNodeByID(partIdx2Areas[idx])->m_label << "')\n");
}

// --------------------------------------------------------
@@ -1814,7 +1814,7 @@ void CHMTSLAM::LSLAM_process_message_from_AA(const TMessageLSLAMfromAA& myMsg)
mrpt::system::LVL_INFO,
"[LSLAM_proc_msg_AA] LMH->changeCoordinateOrigin %lu->%lu took %f "
"ms\n",
poseID_origin, new_poseID_origin, tt.Tac() * 1000);
poseID_origin, new_poseID_origin, tt.Tac() * 1000);
}
else if (areasDelayedMetricMapsInsertion.size())
{
@@ -1851,7 +1851,7 @@ void CHMTSLAM::LSLAM_process_message_from_AA(const TMessageLSLAMfromAA& myMsg)
logFmt(
mrpt::system::LVL_INFO,
"[LSLAM_proc_msg_AA] areasDelayedMetricMapsInsertion took %f ms\n",
tt.Tac() * 1000);
tt.Tac() * 1000);
}

if (false)
@@ -565,7 +565,7 @@ void CRangeBearingKFSLAM::OnObservationJacobians(
*/
void CRangeBearingKFSLAM::OnGetObservationsAndDataAssociation(
vector_KFArray_OBS& Z, std::vector<int>& data_association,
const vector_KFArray_OBS& all_predictions, const KFMatrix& S,
const vector_KFArray_OBS& all_predictions, const KFMatrix& S,
const std::vector<size_t>& lm_indices_in_S, const KFMatrix_OxO& R)
{
MRPT_START
@@ -673,7 +673,7 @@ void CRangeBearingKFSLAM::OnGetObservationsAndDataAssociation(
const size_t i = lm_indices_in_S[q];
for (size_t w = 0; w < obs_size; w++)
m_last_data_association.Y_pred_means.get_unsafe(q, w) =
all_predictions[i][w];
all_predictions[i][w];
m_last_data_association.predictions_IDs.push_back(
i); // for the conversion of indices...
}
@@ -524,7 +524,7 @@ void CRangeBearingKFSLAM2D::OnObservationJacobians(
*/
void CRangeBearingKFSLAM2D::OnGetObservationsAndDataAssociation(
vector_KFArray_OBS& Z, std::vector<int>& data_association,
const vector_KFArray_OBS& all_predictions, const KFMatrix& S,
const vector_KFArray_OBS& all_predictions, const KFMatrix& S,
const std::vector<size_t>& lm_indices_in_S, const KFMatrix_OxO& R)
{
MRPT_START
@@ -625,7 +625,7 @@ void CRangeBearingKFSLAM2D::OnGetObservationsAndDataAssociation(
idx_pred++)
{
const KFArray_OBS& lm_mu =
all_predictions[lm_indices_in_S[idx_pred]];
all_predictions[lm_indices_in_S[idx_pred]];

const size_t base_idx_in_S = obs_size * idx_pred;
KFMatrix_OxO lm_cov;
@@ -680,7 +680,7 @@ void CRangeBearingKFSLAM2D::OnGetObservationsAndDataAssociation(
const size_t i = lm_indices_in_S[q];
for (size_t w = 0; w < obs_size; w++)
m_last_data_association.Y_pred_means.get_unsafe(q, w) =
all_predictions[i][w];
all_predictions[i][w];
m_last_data_association.predictions_IDs.push_back(
i); // for the conversion of indices...
}

0 comments on commit d01bdcb

Please sign in to comment.