Skip to content
Snippets Groups Projects
Commit bff2bb46 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

Changes in order to simplify the addition of new constraints. Now they 

must be added in
ceres_wrapper/derived_constraint_auto_diff_cost_funcion.h instead of in
ceres_manager.cpp
All tested by test_wolf_autodiffwrapper.cpp
parent 1e0adebe
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/CMakeLists.txt
+ 1
− 0
View file @ bff2bb46
......@@ -197,6 +197,7 @@ IF (Ceres_FOUND)
ceres_wrapper/ceres_manager.h
ceres_wrapper/auto_diff_cost_function_wrapper.h
ceres_wrapper/auto_diff_cost_function_wrapper_base.h
ceres_wrapper/derived_constraint_auto_diff_cost_function.h
ceres_wrapper/cost_function_wrapper.h
ceres_wrapper/local_parametrization_wrapper.h )
SET(SRCS_WRAPPER
......
src/ceres_wrapper/ceres_manager.cpp
+ 39
− 76
View file @ bff2bb46
......@@ -271,13 +271,13 @@ ceres::CostFunction* CeresManager::createCostFunction(ConstraintBase* _corrPtr,
if (_corrPtr->getJacobianMethod() == JAC_ANALYTIC)
return new CostFunctionWrapper((ConstraintAnalytic*)_corrPtr);
switch (_corrPtr->getType())
{
case CTR_GPS_FIX_2D:
{
if (_self_auto_diff)
return new AutoDiffCostFunctionWrapper<ConstraintGPS2D>(_corrPtr);
else
if (_self_auto_diff)
return derivedConstraintAutoDiffCostFunction(_corrPtr);
else
{
switch (_corrPtr->getType())
{
case CTR_GPS_FIX_2D:
{
ConstraintGPS2D* specific_ptr = (ConstraintGPS2D*)(_corrPtr);
return new ceres::AutoDiffCostFunction<ConstraintGPS2D,
......@@ -293,13 +293,7 @@ ceres::CostFunction* CeresManager::createCostFunction(ConstraintBase* _corrPtr,
specific_ptr->block8Size,
specific_ptr->block9Size>(specific_ptr);
}
break;
}
case CTR_FIX:
{
if (_self_auto_diff)
return new AutoDiffCostFunctionWrapper<ConstraintFix>(_corrPtr);
else
case CTR_FIX:
{
ConstraintFix* specific_ptr = (ConstraintFix*)(_corrPtr);
return new ceres::AutoDiffCostFunction<ConstraintFix,
......@@ -315,13 +309,7 @@ ceres::CostFunction* CeresManager::createCostFunction(ConstraintBase* _corrPtr,
specific_ptr->block8Size,
specific_ptr->block9Size>(specific_ptr);
}
break;
}
case CTR_ODOM_2D:
{
if (_self_auto_diff)
return new AutoDiffCostFunctionWrapper<ConstraintOdom2D>(_corrPtr);
else
case CTR_ODOM_2D:
{
ConstraintOdom2D* specific_ptr = (ConstraintOdom2D*)(_corrPtr);
return new ceres::AutoDiffCostFunction<ConstraintOdom2D,
......@@ -337,13 +325,7 @@ ceres::CostFunction* CeresManager::createCostFunction(ConstraintBase* _corrPtr,
specific_ptr->block8Size,
specific_ptr->block9Size>(specific_ptr);
}
break;
}
case CTR_CORNER_2D:
{
if (_self_auto_diff)
return new AutoDiffCostFunctionWrapper<ConstraintCorner2D>(_corrPtr);
else
case CTR_CORNER_2D:
{
ConstraintCorner2D* specific_ptr = (ConstraintCorner2D*)(_corrPtr);
return new ceres::AutoDiffCostFunction<ConstraintCorner2D,
......@@ -359,13 +341,7 @@ ceres::CostFunction* CeresManager::createCostFunction(ConstraintBase* _corrPtr,
specific_ptr->block8Size,
specific_ptr->block9Size>(specific_ptr);
}
break;
}
case CTR_CONTAINER:
{
if (_self_auto_diff)
return new AutoDiffCostFunctionWrapper<ConstraintContainer>(_corrPtr);
else
case CTR_CONTAINER:
{
ConstraintContainer* specific_ptr = (ConstraintContainer*)(_corrPtr);
return new ceres::AutoDiffCostFunction<ConstraintContainer,
......@@ -381,55 +357,42 @@ ceres::CostFunction* CeresManager::createCostFunction(ConstraintBase* _corrPtr,
specific_ptr->block8Size,
specific_ptr->block9Size>(specific_ptr);
}
break;
}
case CTR_GPS_PR_3D:
{
if (_self_auto_diff)
return new AutoDiffCostFunctionWrapper<ConstraintGPSPseudorange3D>(_corrPtr);
else
case CTR_GPS_PR_3D:
{
ConstraintGPSPseudorange3D* specific_ptr = (ConstraintGPSPseudorange3D*)(_corrPtr);
return new ceres::AutoDiffCostFunction<ConstraintGPSPseudorange3D,
specific_ptr->measurementSize,
specific_ptr->block0Size,
specific_ptr->block1Size,
specific_ptr->block2Size,
specific_ptr->block3Size,
specific_ptr->block4Size,
specific_ptr->block5Size,
specific_ptr->block6Size,
specific_ptr->block7Size,
specific_ptr->block8Size,
specific_ptr->block9Size>(specific_ptr);
specific_ptr->measurementSize,
specific_ptr->block0Size,
specific_ptr->block1Size,
specific_ptr->block2Size,
specific_ptr->block3Size,
specific_ptr->block4Size,
specific_ptr->block5Size,
specific_ptr->block6Size,
specific_ptr->block7Size,
specific_ptr->block8Size,
specific_ptr->block9Size>(specific_ptr);
}
break;
}
case CTR_GPS_PR_2D:
{
if (_self_auto_diff)
return new AutoDiffCostFunctionWrapper<ConstraintGPSPseudorange2D>(_corrPtr);
else
case CTR_GPS_PR_2D:
{
ConstraintGPSPseudorange2D* specific_ptr = (ConstraintGPSPseudorange2D*)(_corrPtr);
return new ceres::AutoDiffCostFunction<ConstraintGPSPseudorange2D,
specific_ptr->measurementSize,
specific_ptr->block0Size,
specific_ptr->block1Size,
specific_ptr->block2Size,
specific_ptr->block3Size,
specific_ptr->block4Size,
specific_ptr->block5Size,
specific_ptr->block6Size,
specific_ptr->block7Size,
specific_ptr->block8Size,
specific_ptr->block9Size>(specific_ptr);
specific_ptr->measurementSize,
specific_ptr->block0Size,
specific_ptr->block1Size,
specific_ptr->block2Size,
specific_ptr->block3Size,
specific_ptr->block4Size,
specific_ptr->block5Size,
specific_ptr->block6Size,
specific_ptr->block7Size,
specific_ptr->block8Size,
specific_ptr->block9Size>(specific_ptr);
}
break;
}
default:
std::cout << "Unknown constraint type! Please add it in the CeresWrapper::createCostFunction()" << std::endl;
default:
std::cout << "Unknown constraint type! Please add it in the CeresWrapper::createCostFunction()" << std::endl;
return nullptr;
return nullptr;
}
}
}
src/ceres_wrapper/ceres_manager.h
+ 1
− 9
View file @ bff2bb46
......@@ -7,19 +7,11 @@
#include "glog/logging.h"
//wolf includes
#include "auto_diff_cost_function_wrapper.h"
#include "derived_constraint_auto_diff_cost_function.h"
#include "cost_function_wrapper.h"
#include "local_parametrization_wrapper.h"
#include "../wolf.h"
#include "../state_block.h"
#include "../constraint_sparse.h"
#include "../constraint_fix.h"
#include "../constraint_gps_2D.h"
#include "../constraint_gps_pseudorange_3D.h"
#include "../constraint_gps_pseudorange_2D.h"
#include "../constraint_odom_2D.h"
#include "../constraint_corner_2D.h"
#include "../constraint_container.h"
/** \brief Enumeration of covariance blocks to be computed
*
......
src/ceres_wrapper/derived_constraint_auto_diff_cost_function.h 0 → 100644
+ 52
− 0
View file @ bff2bb46
/*
* ceres.h
*
* Created on: Apr 4, 2016
* Author: jvallve
*/
#ifndef SRC_CERES_WRAPPER_DERIVED_CONSTRAINT_AUTO_DIFF_COST_FUNCTION_H_
#define SRC_CERES_WRAPPER_DERIVED_CONSTRAINT_AUTO_DIFF_COST_FUNCTION_H_
#include "auto_diff_cost_function_wrapper.h"
// Constraints includes
#include "../constraint_sparse.h"
#include "../constraint_fix.h"
#include "../constraint_gps_2D.h"
#include "../constraint_gps_pseudorange_3D.h"
#include "../constraint_gps_pseudorange_2D.h"
#include "../constraint_odom_2D.h"
#include "../constraint_corner_2D.h"
#include "../constraint_container.h"
ceres::CostFunction* derivedConstraintAutoDiffCostFunction(ConstraintBase* _ctr_ptr)
{
switch (_ctr_ptr->getType())
{
case CTR_GPS_FIX_2D:
return new AutoDiffCostFunctionWrapper<ConstraintGPS2D>(_ctr_ptr);
case CTR_FIX:
return new AutoDiffCostFunctionWrapper<ConstraintFix>(_ctr_ptr);
case CTR_ODOM_2D:
return new AutoDiffCostFunctionWrapper<ConstraintOdom2D>(_ctr_ptr);
case CTR_CORNER_2D:
return new AutoDiffCostFunctionWrapper<ConstraintCorner2D>(_ctr_ptr);
case CTR_CONTAINER:
return new AutoDiffCostFunctionWrapper<ConstraintContainer>(_ctr_ptr);
case CTR_GPS_PR_3D:
return new AutoDiffCostFunctionWrapper<ConstraintGPSPseudorange3D>(_ctr_ptr);
case CTR_GPS_PR_2D:
return new AutoDiffCostFunctionWrapper<ConstraintGPSPseudorange2D>(_ctr_ptr);
default:
throw std::invalid_argument( "Unknown constraint type! Please add it in the file: ceres_wrapper/derived_constraint_auto_diff_cost_function.h" );
}
}
#endif /* SRC_CERES_WRAPPER_DERIVED_CONSTRAINT_AUTO_DIFF_COST_FUNCTION_H_ */
src/examples/test_wolf_autodiffwrapper.cpp
+ 1
− 1
View file @ bff2bb46
......@@ -278,7 +278,7 @@ int main(int argc, char** argv)
ceres_manager_ceres_diff->update();
double t_update_ceres = ((double) clock() - t1) / CLOCKS_PER_SEC;
t1 = clock();
ceres_manager_wolf_diff->update();
ceres_manager_wolf_diff->update(true);
double t_update_wolf = ((double) clock() - t1) / CLOCKS_PER_SEC;
std::cout << "updated!" << std::endl;
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment