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

analytic constraints and their ceres cost function wrapper

parent 6302a8bc
No related branches found
No related tags found
No related merge requests found
#ifndef TRUNK_SRC_COST_FUNCTION_WRAPPER_H_
#define TRUNK_SRC_COST_FUNCTION_WRAPPER_H_
// WOLF
#include "../wolf.h"
#include "../constraint_analytic.h"
// CERES
#include "ceres/cost_function.h"
class CostFunctionWrapper : public ceres::CostFunction
{
protected:
ConstraintAnalytic* constraint_ptr_;
std::vector<unsigned int> state_blocks_sizes_;
public:
CostFunctionWrapper(ConstraintAnalytic* _constraint_ptr) :
ceres::CostFunction(),
constraint_ptr_(_constraint_ptr),
state_blocks_sizes_(constraint_ptr_->getStateSizes())
{
for (unsigned int i = 0; i < constraint_ptr_->getStatePtrVector().size(); i++)
mutable_parameter_block_sizes()->push_back(state_blocks_sizes_[i]);
set_num_residuals(constraint_ptr_->getSize());
};
virtual ~CostFunctionWrapper()
{
};
virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const
{
// load parameters evaluation value
std::vector<Eigen::Map<const Eigen::VectorXs>> state_blocks_map_;
for (unsigned int i = 0; i < state_blocks_sizes_.size(); i++)
state_blocks_map_.push_back(Eigen::Map<const Eigen::VectorXs>(parameters[i], state_blocks_sizes_[i]));
// residuals
Eigen::Map<Eigen::VectorXs> residuals_map(residuals, constraint_ptr_->getSize());
residuals_map = constraint_ptr_->evaluateResiduals(state_blocks_map_);
// also compute jacobians
if (jacobians != nullptr)
{
std::vector<Eigen::Map<Eigen::MatrixXs>> jacobians_map_;
std::vector<bool> compute_jacobians_(state_blocks_sizes_.size());
for (unsigned int i = 0; i < state_blocks_sizes_.size(); i++)
{
compute_jacobians_[i] = (jacobians[i] != nullptr);
if (jacobians[i] != nullptr)
jacobians_map_.push_back(Eigen::Map<Eigen::MatrixXs>(jacobians[i], constraint_ptr_->getSize(), state_blocks_sizes_[i]));
else
jacobians_map_.push_back(Eigen::Map<Eigen::MatrixXs>(nullptr, 0, 0)); //TODO: check if it can be done
}
// evaluate jacobians
constraint_ptr_->evaluateJacobians(state_blocks_map_, jacobians_map_, compute_jacobians_);
}
return true;
}
};
#endif /* TRUNK_SRC_COST_FUNCTION_WRAPPER_H_ */
#include "constraint_analytic.h"
ConstraintAnalytic::ConstraintAnalytic(FeatureBase* _ftr_ptr, ConstraintType _tp, ConstraintStatus _status,
StateBlock* _state0Ptr, StateBlock* _state1Ptr, StateBlock* _state2Ptr, StateBlock* _state3Ptr, StateBlock* _state4Ptr,
StateBlock* _state5Ptr, StateBlock* _state6Ptr, StateBlock* _state7Ptr, StateBlock* _state8Ptr, StateBlock* _state9Ptr ) :
ConstraintBase(_tp, _status),
state_ptr_vector_({_state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr,
_state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr})
{
resizeVectors();
}
ConstraintAnalytic::ConstraintAnalytic(FeatureBase* _ftr_ptr, ConstraintType _tp, FrameBase* _frame_ptr, ConstraintStatus _status,
StateBlock* _state0Ptr, StateBlock* _state1Ptr, StateBlock* _state2Ptr, StateBlock* _state3Ptr, StateBlock* _state4Ptr,
StateBlock* _state5Ptr, StateBlock* _state6Ptr, StateBlock* _state7Ptr, StateBlock* _state8Ptr, StateBlock* _state9Ptr ) :
ConstraintBase(_tp, _frame_ptr, _status),
state_ptr_vector_({_state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr,
_state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr})
{
resizeVectors();
}
ConstraintAnalytic::ConstraintAnalytic(FeatureBase* _ftr_ptr, ConstraintType _tp, FeatureBase* _feature_ptr, ConstraintStatus _status,
StateBlock* _state0Ptr, StateBlock* _state1Ptr, StateBlock* _state2Ptr, StateBlock* _state3Ptr, StateBlock* _state4Ptr,
StateBlock* _state5Ptr, StateBlock* _state6Ptr, StateBlock* _state7Ptr, StateBlock* _state8Ptr, StateBlock* _state9Ptr ) :
ConstraintBase( _tp, _feature_ptr, _status),
state_ptr_vector_({_state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr,
_state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr})
{
resizeVectors();
}
ConstraintAnalytic::ConstraintAnalytic(FeatureBase* _ftr_ptr, ConstraintType _tp, LandmarkBase* _landmark_ptr, ConstraintStatus _status,
StateBlock* _state0Ptr, StateBlock* _state1Ptr, StateBlock* _state2Ptr, StateBlock* _state3Ptr, StateBlock* _state4Ptr,
StateBlock* _state5Ptr, StateBlock* _state6Ptr, StateBlock* _state7Ptr, StateBlock* _state8Ptr, StateBlock* _state9Ptr ) :
ConstraintBase( _tp, _landmark_ptr, _status),
state_ptr_vector_({_state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr,
_state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr})
{
resizeVectors();
}
ConstraintAnalytic::~ConstraintAnalytic()
{
//
}
const std::vector<WolfScalar*> ConstraintAnalytic::getStateBlockPtrVector()
{
assert(state_ptr_vector_.size() > 0 && state_ptr_vector_.size() <= 10 && "Wrong state vector size in constraint, it should be between 1 and 10");
switch (state_ptr_vector_.size())
{
case 1:
{
return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr()});
}
case 2:
{
return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
state_ptr_vector_[1]->getPtr()});
}
case 3:
{
return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
state_ptr_vector_[1]->getPtr(),
state_ptr_vector_[2]->getPtr()});
}
case 4:
{
return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
state_ptr_vector_[1]->getPtr(),
state_ptr_vector_[2]->getPtr(),
state_ptr_vector_[3]->getPtr()});
}
case 5:
{
return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
state_ptr_vector_[1]->getPtr(),
state_ptr_vector_[2]->getPtr(),
state_ptr_vector_[3]->getPtr(),
state_ptr_vector_[4]->getPtr()});
}
case 6:
{
return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
state_ptr_vector_[1]->getPtr(),
state_ptr_vector_[2]->getPtr(),
state_ptr_vector_[3]->getPtr(),
state_ptr_vector_[4]->getPtr(),
state_ptr_vector_[5]->getPtr()});
}
case 7:
{
return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
state_ptr_vector_[1]->getPtr(),
state_ptr_vector_[2]->getPtr(),
state_ptr_vector_[3]->getPtr(),
state_ptr_vector_[4]->getPtr(),
state_ptr_vector_[5]->getPtr(),
state_ptr_vector_[6]->getPtr()});
}
case 8:
{
return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
state_ptr_vector_[1]->getPtr(),
state_ptr_vector_[2]->getPtr(),
state_ptr_vector_[3]->getPtr(),
state_ptr_vector_[4]->getPtr(),
state_ptr_vector_[5]->getPtr(),
state_ptr_vector_[6]->getPtr(),
state_ptr_vector_[7]->getPtr()});
}
case 9:
{
return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
state_ptr_vector_[1]->getPtr(),
state_ptr_vector_[2]->getPtr(),
state_ptr_vector_[3]->getPtr(),
state_ptr_vector_[4]->getPtr(),
state_ptr_vector_[5]->getPtr(),
state_ptr_vector_[6]->getPtr(),
state_ptr_vector_[7]->getPtr(),
state_ptr_vector_[8]->getPtr()});
}
case 10:
{
return std::vector<WolfScalar*>({state_ptr_vector_[0]->getPtr(),
state_ptr_vector_[1]->getPtr(),
state_ptr_vector_[2]->getPtr(),
state_ptr_vector_[3]->getPtr(),
state_ptr_vector_[4]->getPtr(),
state_ptr_vector_[5]->getPtr(),
state_ptr_vector_[6]->getPtr(),
state_ptr_vector_[7]->getPtr(),
state_ptr_vector_[8]->getPtr(),
state_ptr_vector_[9]->getPtr()});
}
}
return std::vector<WolfScalar*>(0); //Not going to happen
}
const std::vector<StateBlock*> ConstraintAnalytic::getStatePtrVector() const
{
return state_ptr_vector_;
}
std::vector<unsigned int> ConstraintAnalytic::getStateSizes() const
{
return state_block_sizes_vector_;
}
JacobianMethod ConstraintAnalytic::getJacobianMethod() const
{
return ANALYTIC;
}
void ConstraintAnalytic::print(unsigned int _ntabs, std::ostream& _ost) const
{
NodeLinked::printSelf(_ntabs, _ost);
for (unsigned int ii = 0; ii<state_block_sizes_vector_.size(); ii++)
{
printTabs(_ntabs);
_ost << "block " << ii << ": ";
for (unsigned int jj = 0; jj<state_block_sizes_vector_.at(ii); jj++)
_ost << *(state_ptr_vector_.at(ii)->getPtr()+jj) << " ";
_ost << std::endl;
}
}
void ConstraintAnalytic::resizeVectors()
{
for (unsigned int ii = 1; ii<state_ptr_vector_.size(); ii++)
{
if (state_ptr_vector_.at(ii) != nullptr)
state_block_sizes_vector_.push_back(state_ptr_vector_.at(ii)->getSize());
else
{
state_ptr_vector_.resize(ii);
break;
}
}
}
#ifndef CONSTRAINT_ODOM_2D_ANALYTIC_H_
#define CONSTRAINT_ODOM_2D_ANALYTIC_H_
//Wolf includes
#include "wolf.h"
#include "constraint_analytic.h"
class ConstraintOdom2DAnalytic : public ConstraintAnalytic
{
public:
ConstraintOdom2DAnalytic(FeatureBase* _ftr_ptr, FrameBase* _frame_ptr, ConstraintStatus _status = CTR_ACTIVE) :
ConstraintAnalytic(_ftr_ptr, CTR_ODOM_2D, _frame_ptr, _status, _frame_ptr->getPPtr(), _frame_ptr->getOPtr(), _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr())
{
//
}
/** \brief Default destructor (not recommended)
*
* Default destructor (please use destruct() instead of delete for guaranteeing the wolf tree integrity)
*
**/
virtual ~ConstraintOdom2DAnalytic()
{
//
}
/** \brief Returns the constraint residual size
*
* Returns the constraint residual size
*
**/
virtual unsigned int getSize() const
{
return 3;
}
/** \brief Returns the residual evaluated in the states provided
*
* Returns the residual evaluated in the states provided in std::vector of mapped Eigen::VectorXs
*
**/
virtual Eigen::VectorXs evaluateResiduals(const std::vector<Eigen::Map<const Eigen::VectorXs>>& _st_vector) const
{
Eigen::VectorXs residual(3);
Eigen::VectorXs expected_measurement(3);
// Expected measurement
Eigen::Matrix2s R = Eigen::Rotation2D<WolfScalar>(-_st_vector[1](0)).matrix();
expected_measurement.head(2) = R * (_st_vector[2]-_st_vector[0]); // rotar menys l'angle de primer (-_o1)
expected_measurement(2) = _st_vector[3](0) - _st_vector[1](0);
// Residual
residual = expected_measurement - getMeasurement();
while (residual(2) > M_PI)
residual(2) = residual(2) - 2*M_PI;
while (residual(2) <= -M_PI)
residual(2) = residual(2) + 2*M_PI;
residual = getMeasurementSquareRootInformation() * residual;
return residual;
}
/** \brief Returns the jacobians evaluated in the states provided
*
* Returns the jacobians evaluated in the states provided in std::vector of mapped Eigen::VectorXs.
* IMPORTANT: only fill the jacobians of the state blocks specified in _compute_jacobian.
*
* \param _st_vector is a vector containing the mapped eigen vectors of all state blocks involved in the constraint
* \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
* \param _compute_jacobian is a vector that specifies whether the ith jacobian sould be computed or not
*
**/
virtual void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXs>>& _st_vector, std::vector<Eigen::Map<Eigen::MatrixXs>>& jacobians, const std::vector<bool>& _compute_jacobian) const
{
jacobians[0] << -cos(_st_vector[1](0)), -sin(_st_vector[1](0)),
sin(_st_vector[1](0)), -cos(_st_vector[1](0)),
0, 0;
jacobians[0] = getMeasurementSquareRootInformation() * jacobians[0];
jacobians[1] << -(_st_vector[2](0) - _st_vector[0](0)) * sin(_st_vector[1](0)) + (_st_vector[2](1) - _st_vector[0](1)) * cos(_st_vector[1](0)),
-(_st_vector[2](0) - _st_vector[0](0)) * cos(_st_vector[1](0)) - (_st_vector[2](1) - _st_vector[0](1)) * sin(_st_vector[1](0)),
-1;
jacobians[1] = getMeasurementSquareRootInformation() * jacobians[0];
jacobians[2] << cos(_st_vector[1](0)), sin(_st_vector[1](0)),
-sin(_st_vector[1](0)),cos(_st_vector[1](0)),
0, 0;
jacobians[2] = getMeasurementSquareRootInformation() * jacobians[0];
jacobians[3] << 0, 0, 1;
jacobians[3] = getMeasurementSquareRootInformation() * jacobians[0];
}
/** \brief Returns the jacobians computation method
*
* Returns the jacobians computation method
*
**/
virtual JacobianMethod getJacobianMethod() const
{
return AUTO;
}
};
#endif
// Testing creating wolf tree from imported .graph file
//C includes for sleep, time and main args
#include "unistd.h"
//std includes
#include <iostream>
#include <memory>
#include <random>
#include <cmath>
#include <queue>
//Wolf includes
#include "wolf_manager.h"
#include "capture_void.h"
#include "ceres_wrapper/ceres_manager.h"
// EIGEN
//#include <Eigen/CholmodSupport>
// inserts the sparse matrix 'ins' into the sparse matrix 'original' in the place given by 'row' and 'col' integers
void insertSparseBlock(const Eigen::SparseMatrix<WolfScalar>& ins, Eigen::SparseMatrix<WolfScalar>& original, const unsigned int& row, const unsigned int& col)
{
for (int k=0; k<ins.outerSize(); ++k)
for (Eigen::SparseMatrix<WolfScalar>::InnerIterator iti(ins,k); iti; ++iti)
original.coeffRef(iti.row() + row, iti.col() + col) = iti.value();
original.makeCompressed();
}
int main(int argc, char** argv)
{
//Welcome message
std::cout << std::endl << " ========= WOLF IMPORTED .graph TEST ===========" << std::endl << std::endl;
if (argc != 3 || atoi(argv[2]) < 0 )
{
std::cout << "Please call me with: [./test_wolf_imported_graph FILE_PATH MAX_VERTICES], where:" << std::endl;
std::cout << " FILE_PATH is the .graph file path" << std::endl;
std::cout << " MAX_VERTICES max edges to be loaded (0: ALL)" << std::endl;
std::cout << "EXIT due to bad user input" << std::endl << std::endl;
return -1;
}
// auxiliar variables
std::string file_path_ = argv[1];
unsigned int MAX_VERTEX = atoi(argv[2]);
if (MAX_VERTEX == 0) MAX_VERTEX = 1e6;
std::ifstream offLineFile_;
ceres::Solver::Summary summary_autodiff, summary_analytic;
// loading variables
std::map<unsigned int, FrameBase*> index_2_frame_ptr_autodiff;
std::map<unsigned int, FrameBase*> index_2_frame_ptr_analytic;
// Wolf problem
WolfProblem* wolf_problem_autodiff = new WolfProblem();
WolfProblem* wolf_problem_analytic = new WolfProblem();
SensorBase* sensor = new SensorBase(ODOM_2D, new StateBlock(Eigen::VectorXs::Zero(2)), new StateBlock(Eigen::VectorXs::Zero(1)), new StateBlock(Eigen::VectorXs::Zero(2)), 2);
// Ceres wrapper
ceres::Solver::Options ceres_options;
ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH
ceres_options.max_line_search_step_contraction = 1e-3;
ceres_options.max_num_iterations = 1e4;
ceres::Problem::Options problem_options;
problem_options.cost_function_ownership = ceres::TAKE_OWNERSHIP;
problem_options.loss_function_ownership = ceres::TAKE_OWNERSHIP;
problem_options.local_parameterization_ownership = ceres::TAKE_OWNERSHIP;
CeresManager* ceres_manager_autodiff = new CeresManager(wolf_problem_autodiff, problem_options);
CeresManager* ceres_manager_analytic = new CeresManager(wolf_problem_analytic, problem_options);
// load graph from .txt
offLineFile_.open(file_path_.c_str(), std::ifstream::in);
if (offLineFile_.is_open())
{
std::string buffer;
unsigned int j = 0;
// Line by line
while (std::getline(offLineFile_, buffer))
{
//std::cout << "new line:" << buffer << std::endl;
std::string bNum;
unsigned int i = 0;
// VERTEX
if (buffer.at(0) == 'V')
{
//skip rest of VERTEX word
while (buffer.at(i) != ' ') i++;
//skip white spaces
while (buffer.at(i) == ' ') i++;
//vertex index
while (buffer.at(i) != ' ')
bNum.push_back(buffer.at(i++));
unsigned int vertex_index = atoi(bNum.c_str());
bNum.clear();
if (vertex_index <= MAX_VERTEX+1)
{
//skip white spaces
while (buffer.at(i) == ' ') i++;
// vertex pose
Eigen::Vector3s vertex_pose;
// x
while (buffer.at(i) != ' ')
bNum.push_back(buffer.at(i++));
vertex_pose(0) = atof(bNum.c_str());
bNum.clear();
//skip white spaces
while (buffer.at(i) == ' ') i++;
// y
while (buffer.at(i) != ' ')
bNum.push_back(buffer.at(i++));
vertex_pose(1) = atof(bNum.c_str());
bNum.clear();
//skip white spaces
while (buffer.at(i) == ' ') i++;
// theta
while (i < buffer.size() && buffer.at(i) != ' ')
bNum.push_back(buffer.at(i++));
vertex_pose(2) = atof(bNum.c_str());
bNum.clear();
// add frame to problem
FrameBase* vertex_frame_ptr_autodiff = new FrameBase(TimeStamp(0), new StateBlock(vertex_pose.head(2)), new StateBlock(vertex_pose.tail(1)));
FrameBase* vertex_frame_ptr_analytic = new FrameBase(TimeStamp(0), new StateBlock(vertex_pose.head(2)), new StateBlock(vertex_pose.tail(1)));
wolf_problem_autodiff->getTrajectoryPtr()->addFrame(vertex_frame_ptr_autodiff);
wolf_problem_analytic->getTrajectoryPtr()->addFrame(vertex_frame_ptr_analytic);
// store
index_2_frame_ptr_autodiff[vertex_index] = vertex_frame_ptr_autodiff;
index_2_frame_ptr_analytic[vertex_index] = vertex_frame_ptr_analytic;
//std::cout << "Added vertex! index: " << vertex_index << " nodeId: " << vertex_frame_ptr_analytic->nodeId() << std::endl << "pose: " << vertex_pose.transpose() << std::endl;
}
}
// EDGE
else if (buffer.at(0) == 'E')
{
j++;
//skip rest of EDGE word
while (buffer.at(i) != ' ') i++;
//skip white spaces
while (buffer.at(i) == ' ') i++;
//from
while (buffer.at(i) != ' ')
bNum.push_back(buffer.at(i++));
unsigned int edge_old = atoi(bNum.c_str());
bNum.clear();
//skip white spaces
while (buffer.at(i) == ' ') i++;
//to index
while (buffer.at(i) != ' ')
bNum.push_back(buffer.at(i++));
unsigned int edge_new = atoi(bNum.c_str());
bNum.clear();
if (edge_new <= MAX_VERTEX+1 && edge_old <= MAX_VERTEX+1 )
{
//skip white spaces
while (buffer.at(i) == ' ') i++;
// edge vector
Eigen::Vector3s edge_vector;
// x
while (buffer.at(i) != ' ')
bNum.push_back(buffer.at(i++));
edge_vector(0) = atof(bNum.c_str());
bNum.clear();
//skip white spaces
while (buffer.at(i) == ' ') i++;
// y
while (buffer.at(i) != ' ')
bNum.push_back(buffer.at(i++));
edge_vector(1) = atof(bNum.c_str());
bNum.clear();
//skip white spaces
while (buffer.at(i) == ' ') i++;
// theta
while (buffer.at(i) != ' ')
bNum.push_back(buffer.at(i++));
edge_vector(2) = atof(bNum.c_str());
bNum.clear();
//skip white spaces
while (buffer.at(i) == ' ') i++;
// edge covariance
Eigen::Matrix3s edge_information;
// xx
while (buffer.at(i) != ' ')
bNum.push_back(buffer.at(i++));
edge_information(0,0) = atof(bNum.c_str());
bNum.clear();
//skip white spaces
while (buffer.at(i) == ' ') i++;
// xy
while (buffer.at(i) != ' ')
bNum.push_back(buffer.at(i++));
edge_information(0,1) = atof(bNum.c_str());
edge_information(1,0) = atof(bNum.c_str());
bNum.clear();
//skip white spaces
while (buffer.at(i) == ' ') i++;
// yy
while (buffer.at(i) != ' ')
bNum.push_back(buffer.at(i++));
edge_information(1,1) = atof(bNum.c_str());
bNum.clear();
//skip white spaces
while (buffer.at(i) == ' ') i++;
// thetatheta
while (buffer.at(i) != ' ')
bNum.push_back(buffer.at(i++));
edge_information(2,2) = atof(bNum.c_str());
bNum.clear();
//skip white spaces
while (buffer.at(i) == ' ') i++;
// xtheta
while (buffer.at(i) != ' ')
bNum.push_back(buffer.at(i++));
edge_information(0,2) = atof(bNum.c_str());
edge_information(2,0) = atof(bNum.c_str());
bNum.clear();
//skip white spaces
while (buffer.at(i) == ' ') i++;
// ytheta
while (i < buffer.size() && buffer.at(i) != ' ')
bNum.push_back(buffer.at(i++));
edge_information(1,2) = atof(bNum.c_str());
edge_information(2,1) = atof(bNum.c_str());
bNum.clear();
// add capture, feature and constraint to problem
FeatureBase* feature_ptr_autodiff = new FeatureBase(edge_vector, edge_information.inverse());
CaptureVoid* capture_ptr_autodiff = new CaptureVoid(TimeStamp(0), sensor);
assert(index_2_frame_ptr_autodiff.find(edge_old) != index_2_frame_ptr_autodiff.end() && "edge from vertex not added!");
FrameBase* frame_old_ptr_autodiff = index_2_frame_ptr_autodiff[edge_old];
assert(index_2_frame_ptr_autodiff.find(edge_new) != index_2_frame_ptr_autodiff.end() && "edge to vertex not added!");
FrameBase* frame_new_ptr_autodiff = index_2_frame_ptr_autodiff[edge_new];
frame_new_ptr_autodiff->addCapture(capture_ptr_autodiff);
capture_ptr_autodiff->addFeature(feature_ptr_autodiff);
ConstraintOdom2D* constraint_ptr_autodiff = new ConstraintOdom2D(feature_ptr_autodiff, frame_old_ptr_autodiff);
feature_ptr_autodiff->addConstraintFrom(constraint_ptr_autodiff);
//std::cout << "Added autodiff edge! " << constraint_ptr_autodiff->nodeId() << " from vertex " << constraint_ptr_autodiff->getCapturePtr()->getFramePtr()->nodeId() << " to " << constraint_ptr_autodiff->getFrameOtherPtr()->nodeId() << std::endl;
// add capture, feature and constraint to problem
FeatureBase* feature_ptr_analytic = new FeatureBase(edge_vector, edge_information.inverse());
CaptureVoid* capture_ptr_analytic = new CaptureVoid(TimeStamp(0), sensor);
assert(index_2_frame_ptr_analytic.find(edge_old) != index_2_frame_ptr_analytic.end() && "edge from vertex not added!");
FrameBase* frame_old_ptr_analytic = index_2_frame_ptr_analytic[edge_old];
assert(index_2_frame_ptr_analytic.find(edge_new) != index_2_frame_ptr_analytic.end() && "edge to vertex not added!");
FrameBase* frame_new_ptr_analytic = index_2_frame_ptr_analytic[edge_new];
frame_new_ptr_analytic->addCapture(capture_ptr_analytic);
capture_ptr_analytic->addFeature(feature_ptr_analytic);
ConstraintOdom2DAnalytic* constraint_ptr_analytic = new ConstraintOdom2DAnalytic(feature_ptr_analytic, frame_old_ptr_analytic);
feature_ptr_analytic->addConstraintFrom(constraint_ptr_analytic);
//std::cout << "Added analytic edge! " << constraint_ptr_analytic->nodeId() << " from vertex " << constraint_ptr_analytic->getCapturePtr()->getFramePtr()->nodeId() << " to " << constraint_ptr_analytic->getFrameOtherPtr()->nodeId() << std::endl;
//std::cout << "vector " << constraint_ptr_analytic->getMeasurement().transpose() << std::endl;
//std::cout << "information " << std::endl << edge_information << std::endl;
//std::cout << "covariance " << std::endl << constraint_ptr_analytic->getMeasurementCovariance() << std::endl;
}
}
else
assert("unknown line");
}
printf("\nGraph loaded!\n");
}
else
printf("\nError opening file\n");
// PRIOR
FrameBase* first_frame_autodiff = wolf_problem_autodiff->getTrajectoryPtr()->getFrameListPtr()->front();
FrameBase* first_frame_analytic = wolf_problem_analytic->getTrajectoryPtr()->getFrameListPtr()->front();
CaptureFix* initial_covariance_autodiff = new CaptureFix(TimeStamp(0), new SensorBase(ABSOLUTE_POSE, nullptr, nullptr, nullptr, 0), first_frame_autodiff->getState(), Eigen::Matrix3s::Identity() * 0.01);
CaptureFix* initial_covariance_analytic = new CaptureFix(TimeStamp(0), new SensorBase(ABSOLUTE_POSE, nullptr, nullptr, nullptr, 0), first_frame_analytic->getState(), Eigen::Matrix3s::Identity() * 0.01);
first_frame_autodiff->addCapture(initial_covariance_autodiff);
first_frame_analytic->addCapture(initial_covariance_analytic);
initial_covariance_autodiff->process();
initial_covariance_analytic->process();
//std::cout << "initial covariance: constraint " << initial_covariance_analytic->getFeatureListPtr()->front()->getConstraintFromListPtr()->front()->nodeId() << std::endl << initial_covariance_analytic->getFeatureListPtr()->front()->getMeasurementCovariance() << std::endl;
// BUILD SOLVER PROBLEM
std::cout << "updating ceres..." << std::endl;
ceres_manager_autodiff->update();
ceres_manager_analytic->update();
std::cout << "updated!" << std::endl;
// SOLVING PROBLEMS
std::cout << "solving..." << std::endl;
std::cout << "ANALYTIC -----------------------------------" << std::endl;
summary_analytic = ceres_manager_analytic->solve(ceres_options);
std::cout << summary_analytic.FullReport() << std::endl;
std::cout << "AUTODIFF -----------------------------------" << std::endl;
summary_autodiff = ceres_manager_autodiff->solve(ceres_options);
std::cout << summary_autodiff.FullReport() << std::endl;
delete wolf_problem_autodiff; //not necessary to delete anything more, wolf will do it!
std::cout << "wolf_problem_ deleted!" << std::endl;
delete ceres_manager_autodiff;
delete ceres_manager_analytic;
std::cout << "ceres_manager deleted!" << std::endl;
//End message
std::cout << " =========================== END ===============================" << std::endl << std::endl;
//exit
return 0;
}
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