Add a parameter block to the map
| 253 | |
| 254 | // Add a parameter block to the map |
| 255 | bool Map::addParameterBlock(std::shared_ptr<okvis::ceres::ParameterBlock> parameterBlock, |
| 256 | int parameterization, |
| 257 | const int /*group*/) { |
| 258 | // check Id availability |
| 259 | if (parameterBlockExists(parameterBlock->id())) { |
| 260 | return false; |
| 261 | } |
| 262 | |
| 263 | id2ParameterBlock_Map_.insert( |
| 264 | std::pair<uint64_t, std::shared_ptr<okvis::ceres::ParameterBlock> >(parameterBlock->id(), parameterBlock)); |
| 265 | |
| 266 | // also add to ceres problem |
| 267 | switch (parameterization) { |
| 268 | case Parameterization::Trivial: { |
| 269 | problem_->AddParameterBlock(parameterBlock->parameters(), parameterBlock->dimension()); |
| 270 | break; |
| 271 | } |
| 272 | case Parameterization::HomogeneousPoint: { |
| 273 | problem_->AddParameterBlock( |
| 274 | parameterBlock->parameters(), parameterBlock->dimension(), &homogeneousPointManifold_); |
| 275 | parameterBlock->setManifoldPtr(&homogeneousPointManifold_); |
| 276 | break; |
| 277 | } |
| 278 | // @Sharmin |
| 279 | // case Parameterization::Sonar: { |
| 280 | // problem_->AddParameterBlock(parameterBlock->parameters(), parameterBlock->dimension(), &sonarManifold_); |
| 281 | // parameterBlock->setManifoldPtr(&sonarManifold_); |
| 282 | // break; |
| 283 | // } |
| 284 | case Parameterization::Pose6d: { |
| 285 | problem_->AddParameterBlock(parameterBlock->parameters(), parameterBlock->dimension(), &poseManifold_); |
| 286 | parameterBlock->setManifoldPtr(&poseManifold_); |
| 287 | break; |
| 288 | } |
| 289 | case Parameterization::Pose3d: { |
| 290 | problem_->AddParameterBlock(parameterBlock->parameters(), parameterBlock->dimension(), &poseManifold3d_); |
| 291 | parameterBlock->setManifoldPtr(&poseManifold3d_); |
| 292 | break; |
| 293 | } |
| 294 | case Parameterization::Pose4d: { |
| 295 | problem_->AddParameterBlock(parameterBlock->parameters(), parameterBlock->dimension(), &poseManifold4d_); |
| 296 | parameterBlock->setManifoldPtr(&poseManifold4d_); |
| 297 | break; |
| 298 | } |
| 299 | case Parameterization::Pose2d: { |
| 300 | problem_->AddParameterBlock(parameterBlock->parameters(), parameterBlock->dimension(), &poseManifold2d_); |
| 301 | parameterBlock->setManifoldPtr(&poseManifold2d_); |
| 302 | break; |
| 303 | } |
| 304 | default: { |
| 305 | return false; |
| 306 | break; // just for consistency... |
| 307 | } |
| 308 | } |
| 309 | |
| 310 | /*const okvis::ceres::LocalParamizationAdditionalInterfaces* ptr = |
| 311 | dynamic_cast<const okvis::ceres::LocalParamizationAdditionalInterfaces*>( |
| 312 | parameterBlock->ManifoldPtr()); |