Commit c03c8b41 authored by Matt Clarkson's avatar Matt Clarkson

Merge branch '61-alternating'

* 61-alternating: (41 commits)
  Issue #61: Update to version 1.0.1
  Issue #61: Update version number to 1.0.0.
  Issue #61: Update Doxygen on front page.
  Issue #61: Doxygen fixes for red and white ball detector.
  Issue #61: Update Doxygen to include Internal classes.
  Issue #61: Move unused code to Internal/NotUsed, but keep compilingit.
  Issue #61: Move HandEye optimisers to Internal/HandEye.
  Issue #61: Move Stereo stuff to Internal/Stereo.
  Issue #61: Remove unnecessary set of includes.
  Issue #61: Move Tsai stuff to Internal/Tsai.
  Issue #61: Don't do Tsai tests if ITK not found, as Tsai is bad without LM optimiser.
  Issue #61: tweak docs.
  Issue #61: Fixup Apps that were relying on default args to stereo calibrations.
  Issue #61: Remove defaulted args on stereo calibration.
  Issue #61: Tidy up some includes.
  Issue #61: Ensure builds without ITK.
  Issue #61: Move functions ComputeFundamentalMatrixFromCameraCalibration and ComputeStereoExtrinsics.
  Issue #61: Tidy up stereo camera calibration.
  Issue #61: Fix KWStyle errors.
  Issue #61: Tidy up, finalise stereo method.
  ...
parents 52fc7201 2f14ee02
......@@ -30,7 +30,7 @@ foreach(policy ${project_policies})
endif()
endforeach()
project(NiftyCal VERSION 00.00.1)
project(NiftyCal VERSION 1.0.1)
######################################################################
# Setting supported build types. Should ONLY be Release or Debug.
......
......@@ -111,6 +111,7 @@ int main(int argc, char ** argv)
cv::Mat leftToRightTranslation;
cv::Matx21d result;
int flags = 0;
result = niftk::FullStereoCameraCalibration(model,
leftPoints,
......@@ -128,6 +129,7 @@ int main(int argc, char ** argv)
leftToRightTranslation,
essentialMatrix,
fundamentalMatrix,
flags,
false // optimise3D, could be command line arg.
);
......
......@@ -183,6 +183,7 @@ int main(int argc, char ** argv)
leftToRightTranslationVector,
essentialMatrix,
fundamentalMatrix,
flags,
optimise3D
);
......
......@@ -103,7 +103,7 @@ int main(int argc, char ** argv)
cv::Mat leftToRightTranslation;
cv::Matx21d result;
int flags = 0;
result = niftk::FullStereoCameraCalibration(model,
leftPoints,
rightPoints,
......@@ -120,6 +120,7 @@ int main(int argc, char ** argv)
leftToRightTranslation,
essentialMatrix,
fundamentalMatrix,
flags,
false // could be command line arg.
);
......@@ -231,6 +232,7 @@ int main(int argc, char ** argv)
leftToRightTranslation,
essentialMatrix,
fundamentalMatrix,
flags,
false // could be command line arg.
);
......
......@@ -131,6 +131,7 @@ int main(int argc, char ** argv)
cv::Mat leftToRightTranslation;
cv::Matx21d result;
int flags = 0;
result = niftk::FullStereoCameraCalibration(firstModel,
leftPoints,
......@@ -148,6 +149,7 @@ int main(int argc, char ** argv)
leftToRightTranslation,
essentialMatrix,
fundamentalMatrix,
flags,
false // optimise 3D, not needed here.
);
......@@ -285,6 +287,7 @@ int main(int argc, char ** argv)
leftToRightTranslation,
essentialMatrix,
fundamentalMatrix,
flags,
optimise3D
);
......@@ -362,6 +365,7 @@ int main(int argc, char ** argv)
leftToRightTranslation,
essentialMatrix,
fundamentalMatrix,
flags,
optimise3D
);
......
......@@ -16,7 +16,7 @@ set(NIFTYCAL_SRCS
Internal/niftkIterativeCalibrationUtilities.cxx
Internal/niftkTriangulationUtilities.cxx
Internal/niftkCalibrationUtilities.cxx
Internal/niftkTsaiUtilities.cxx
Internal/Tsai/niftkTsaiUtilities.cxx
Detectors/niftkPointDetector.cxx
Detectors/niftkChessboardPointDetector.cxx
Detectors/niftkCirclesPointDetector.cxx
......@@ -53,53 +53,64 @@ if(ITK_FOUND)
list(APPEND NIFTYCAL_SRCS Internal/niftkNonLinearCostFunction.cxx)
list(APPEND NIFTYCAL_SRCS Internal/niftkNonLinearNoIntrinsicsCostFunction.cxx)
list(APPEND NIFTYCAL_SRCS Internal/niftkNonLinearMonoCostFunction.cxx)
list(APPEND NIFTYCAL_SRCS Internal/niftkNonLinearTsaiCostFunction.cxx)
list(APPEND NIFTYCAL_SRCS Internal/niftkNonLinearMaltiHandEyeCostFunction.cxx)
list(APPEND NIFTYCAL_SRCS Internal/niftkNonLinearMaltiHandEyeOptimiser.cxx)
list(APPEND NIFTYCAL_SRCS Internal/niftkNonLinearNDOFHandEyeCostFunction.cxx)
list(APPEND NIFTYCAL_SRCS Internal/niftkNonLinearNDOFHandEyeOptimiser.cxx)
list(APPEND NIFTYCAL_SRCS Internal/niftkNonLinearTsai3ParamOptimiser.cxx)
list(APPEND NIFTYCAL_SRCS Internal/niftkNonLinearTsai3ParamCostFunction.cxx)
list(APPEND NIFTYCAL_SRCS Internal/niftkNonLinearTsai2ParamOptimiser.cxx)
list(APPEND NIFTYCAL_SRCS Internal/niftkNonLinearTsai2ParamCostFunction.cxx)
list(APPEND NIFTYCAL_SRCS Internal/niftkNonLinearTsai5ParamOptimiser.cxx)
list(APPEND NIFTYCAL_SRCS Internal/niftkNonLinearTsai5ParamCostFunction.cxx)
list(APPEND NIFTYCAL_SRCS Internal/niftkNonLinearTsai8ParamOptimiser.cxx)
list(APPEND NIFTYCAL_SRCS Internal/niftkNonLinearTsai8ParamCostFunction.cxx)
list(APPEND NIFTYCAL_SRCS Internal/niftkNonLinearTsai10ParamOptimiser.cxx)
list(APPEND NIFTYCAL_SRCS Internal/niftkNonLinearTsai10ParamCostFunction.cxx)
list(APPEND NIFTYCAL_SRCS Internal/niftkNonLinearTsai11ParamOptimiser.cxx)
list(APPEND NIFTYCAL_SRCS Internal/niftkNonLinearTsai11ParamCostFunction.cxx)
list(APPEND NIFTYCAL_SRCS Internal/niftkNonLinearStereoCostFunction.cxx)
list(APPEND NIFTYCAL_SRCS Internal/niftkNonLinearStereo2DCostFunction.cxx)
list(APPEND NIFTYCAL_SRCS Internal/niftkNonLinearStereoHandEye2DCostFunction.cxx)
list(APPEND NIFTYCAL_SRCS Internal/niftkNonLinearStereoHandEye2DOptimiser.cxx)
list(APPEND NIFTYCAL_SRCS Internal/niftkNonLinearStereoCameraCalibration2DCostFunction.cxx)
list(APPEND NIFTYCAL_SRCS Internal/niftkNonLinearStereoCameraCalibration2DOptimiser.cxx)
list(APPEND NIFTYCAL_SRCS Internal/niftkNonLinearStereo3DCostFunction.cxx)
list(APPEND NIFTYCAL_SRCS Internal/niftkNonLinearStereoExtrinsicsCalibration3DCostFunction.cxx)
list(APPEND NIFTYCAL_SRCS Internal/niftkNonLinearStereoExtrinsicsCalibration3DOptimiser.cxx)
list(APPEND NIFTYCAL_SRCS Internal/Tsai/niftkNonLinearTsaiCostFunction.cxx)
list(APPEND NIFTYCAL_SRCS Internal/Tsai/niftkNonLinearTsai3ParamOptimiser.cxx)
list(APPEND NIFTYCAL_SRCS Internal/Tsai/niftkNonLinearTsai3ParamCostFunction.cxx)
list(APPEND NIFTYCAL_SRCS Internal/Tsai/niftkNonLinearTsai2ParamOptimiser.cxx)
list(APPEND NIFTYCAL_SRCS Internal/Tsai/niftkNonLinearTsai2ParamCostFunction.cxx)
list(APPEND NIFTYCAL_SRCS Internal/Tsai/niftkNonLinearTsai5ParamOptimiser.cxx)
list(APPEND NIFTYCAL_SRCS Internal/Tsai/niftkNonLinearTsai5ParamCostFunction.cxx)
list(APPEND NIFTYCAL_SRCS Internal/Tsai/niftkNonLinearTsai8ParamOptimiser.cxx)
list(APPEND NIFTYCAL_SRCS Internal/Tsai/niftkNonLinearTsai8ParamCostFunction.cxx)
list(APPEND NIFTYCAL_SRCS Internal/Tsai/niftkNonLinearTsai10ParamOptimiser.cxx)
list(APPEND NIFTYCAL_SRCS Internal/Tsai/niftkNonLinearTsai10ParamCostFunction.cxx)
list(APPEND NIFTYCAL_SRCS Internal/Tsai/niftkNonLinearTsai11ParamOptimiser.cxx)
list(APPEND NIFTYCAL_SRCS Internal/Tsai/niftkNonLinearTsai11ParamCostFunction.cxx)
list(APPEND NIFTYCAL_SRCS Internal/Stereo/niftkNonLinearStereoCameraCalibration2DCostFunction.cxx)
list(APPEND NIFTYCAL_SRCS Internal/Stereo/niftkNonLinearStereoCameraCalibration2DOptimiser.cxx)
list(APPEND NIFTYCAL_SRCS Internal/Stereo/niftkNonLinearStereoExtrinsicsCalibration3DCostFunction.cxx)
list(APPEND NIFTYCAL_SRCS Internal/Stereo/niftkNonLinearStereoExtrinsicsCalibration3DOptimiser.cxx)
list(APPEND NIFTYCAL_SRCS Internal/HandEye/niftkNonLinearMaltiHandEyeCostFunction.cxx)
list(APPEND NIFTYCAL_SRCS Internal/HandEye/niftkNonLinearMaltiHandEyeOptimiser.cxx)
list(APPEND NIFTYCAL_SRCS Internal/HandEye/niftkNonLinearNDOFHandEyeCostFunction.cxx)
list(APPEND NIFTYCAL_SRCS Internal/HandEye/niftkNonLinearNDOFHandEyeOptimiser.cxx)
list(APPEND NIFTYCAL_SRCS Internal/HandEye/niftkNonLinearStereoHandEye2DCostFunction.cxx)
list(APPEND NIFTYCAL_SRCS Internal/HandEye/niftkNonLinearStereoHandEye2DOptimiser.cxx)
list(APPEND NIFTYCAL_SRCS Internal/HandEye/niftkNonLinearStereoHandEye3DCostFunction.cxx)
list(APPEND NIFTYCAL_SRCS Internal/HandEye/niftkNonLinearStereoHandEye3DOptimiser.cxx)
list(APPEND NIFTYCAL_SRCS Internal/NotUsed/niftkNonLinearStereoIntrinsicsCalibration3DCostFunction.cxx)
list(APPEND NIFTYCAL_SRCS Internal/NotUsed/niftkNonLinearStereoIntrinsicsCalibration3DOptimiser.cxx)
list(APPEND NIFTYCAL_SRCS Internal/NotUsed/niftkNonLinearMonoCostFunction3D.cxx)
list(APPEND NIFTYCAL_SRCS Internal/NotUsed/niftkNonLinearMonoCameraCalibration3DOptimiser.cxx)
list(APPEND NIFTYCAL_SRCS Internal/NotUsed/niftkNonLinearMonoCameraCalibration3DCostFunction.cxx)
list(APPEND NIFTYCAL_SRCS Internal/niftkNonLinearStereoIntrinsicsCalibration3DCostFunction.cxx)
list(APPEND NIFTYCAL_SRCS Internal/niftkNonLinearStereoIntrinsicsCalibration3DOptimiser.cxx)
endif()
list(APPEND NIFTYCAL_SRCS Internal/niftkNonLinearStereoHandEye3DCostFunction.cxx)
list(APPEND NIFTYCAL_SRCS Internal/niftkNonLinearStereoHandEye3DOptimiser.cxx)
endif()
###############################################################################
# Start of Highly Experimental Section: Do not use.
###############################################################################
#list(APPEND CMAKE_PREFIX_PATH /usr/local/lib/cmake/Ceres/)
#list(APPEND CMAKE_PREFIX_PATH /Users/mattclarkson/build/CMakeCatchTemplate-SuperBuild-Qt5/gflags/install)
#list(APPEND CMAKE_PREFIX_PATH /Users/mattclarkson/build/CMakeCatchTemplate-SuperBuild-Qt5/glog/install)
#
#find_package(Ceres REQUIRED)
#include_directories(${CERES_INCLUDE_DIRS})
#
#list(APPEND NIFTYCAL_SRCS Internal/niftkNonLinearCeresStereoOptimiser.cxx)
#list(APPEND NIFTYCAL_SRCS Internal/niftkNonLinearCeresMonoOptimiser.cxx)
#
#include_directories(/Users/mattclarkson/install/gsl-2.5/include)
###############################################################################
# End of Highly Experimental Section: Do not use.
###############################################################################
###############################################################################
# Experimental: Try to calibrate based on rendering the model.
......@@ -129,6 +140,6 @@ if(BUILD_SHARED_LIBS)
endif(WIN32)
endif(BUILD_SHARED_LIBS)
target_link_libraries(niftycal PRIVATE ${ALL_THIRD_PARTY_LIBRARIES})
target_link_libraries(niftycal PRIVATE ${CERES_LIBRARIES} ${ALL_THIRD_PARTY_LIBRARIES} /Users/mattclarkson/install/gsl-2.5/lib/libgsl.a)
add_subdirectory(Apps)
......@@ -21,7 +21,7 @@
namespace niftk
{
/**
* \class BallDetector
* \class RedBallDetector
* \brief Thresholds red things, so base class can fit circles.
*
* This detector is not thread safe.
......
......@@ -21,8 +21,8 @@
namespace niftk
{
/**
* \class BallDetector
* \brief Thresholds red things, so base class can fit circles.
* \class WhiteBallDetector
* \brief Thresholds white things, so base class can fit circles.
*
* This detector is not thread safe.
*
......
......@@ -13,7 +13,7 @@
=============================================================================*/
#include "niftkNonLinearMaltiHandEyeCostFunction.h"
#include "niftkCalibrationUtilities_p.h"
#include <Internal/niftkCalibrationUtilities_p.h>
#include <niftkNiftyCalExceptionMacro.h>
#include <niftkMatrixUtilities.h>
#include <niftkPointUtilities.h>
......
......@@ -15,7 +15,7 @@
#ifndef niftkNonLinearMaltiHandEyeCostFunction_h
#define niftkNonLinearMaltiHandEyeCostFunction_h
#include "niftkNonLinearMonoCostFunction.h"
#include <Internal/niftkNonLinearMonoCostFunction.h>
namespace niftk
{
......
......@@ -13,7 +13,7 @@
=============================================================================*/
#include "niftkNonLinearNDOFHandEyeCostFunction.h"
#include "niftkCalibrationUtilities_p.h"
#include <Internal/niftkCalibrationUtilities_p.h>
#include <niftkNiftyCalExceptionMacro.h>
#include <niftkMatrixUtilities.h>
#include <niftkPointUtilities.h>
......
......@@ -15,8 +15,8 @@
#ifndef niftkNonLinearNDOFHandEyeCostFunction_h
#define niftkNonLinearNDOFHandEyeCostFunction_h
#include "niftkNonLinearNoIntrinsicsCostFunction.h"
#include "niftkNonLinearMonoCostFunction.h"
#include <Internal/niftkNonLinearNoIntrinsicsCostFunction.h>
#include <Internal/niftkNonLinearMonoCostFunction.h>
namespace niftk
{
......
......@@ -13,7 +13,7 @@
=============================================================================*/
#include "niftkNonLinearStereoHandEye2DCostFunction.h"
#include "niftkCalibrationUtilities_p.h"
#include <Internal/niftkCalibrationUtilities_p.h>
#include <niftkNiftyCalExceptionMacro.h>
#include <niftkMatrixUtilities.h>
#include <niftkPointUtilities.h>
......
......@@ -15,8 +15,8 @@
#ifndef niftkNonLinearStereoHandEye2DCostFunction_h
#define niftkNonLinearStereoHandEye2DCostFunction_h
#include "niftkNonLinearStereo2DCostFunction.h"
#include "niftkNonLinearNoIntrinsicsCostFunction.h"
#include <Internal/niftkNonLinearStereo2DCostFunction.h>
#include <Internal/niftkNonLinearNoIntrinsicsCostFunction.h>
namespace niftk
{
......
......@@ -13,7 +13,7 @@
=============================================================================*/
#include "niftkNonLinearStereoHandEye3DCostFunction.h"
#include "niftkCalibrationUtilities_p.h"
#include <Internal/niftkCalibrationUtilities_p.h>
#include <niftkNiftyCalExceptionMacro.h>
#include <niftkMatrixUtilities.h>
#include <niftkPointUtilities.h>
......
......@@ -15,8 +15,8 @@
#ifndef niftkNonLinearStereoHandEye3DCostFunction_h
#define niftkNonLinearStereoHandEye3DCostFunction_h
#include "niftkNonLinearStereo3DCostFunction.h"
#include "niftkNonLinearNoIntrinsicsCostFunction.h"
#include <Internal/niftkNonLinearStereo3DCostFunction.h>
#include <Internal/niftkNonLinearNoIntrinsicsCostFunction.h>
namespace niftk
{
......
/*=============================================================================
NiftyCal: A software package for camera calibration.
Copyright (c) University College London (UCL). All rights reserved.
This software is distributed WITHOUT ANY WARRANTY; without even
the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
PURPOSE.
See LICENSE.txt in the top level directory for details.
=============================================================================*/
#include "niftkNonLinearCeresMonoOptimiser.h"
#include <niftkMatrixUtilities.h>
#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <iostream>
#include <vector>
namespace niftk
{
/**
* \brief Internal function to compute the re-projection point on one camera.
*/
template <typename T>
void ProjectToPoint(const T* const intrinsic,
const T* const distortion,
const T* const cameraPoint,
T& x,
T& y
)
{
// Normalise
T xp = cameraPoint[0] / cameraPoint[2];
T yp = cameraPoint[1] / cameraPoint[2];
// Apply distortion
// distortion[0] = k1
// distortion[1] = k2
// distortion[2] = p1
// distortion[3] = p2
// distortion[4] = k3
T r2 = xp*xp + yp*yp;
T xpd = xp * (T(1) + distortion[0]*r2 + distortion[1]*r2*r2 + distortion[4]*r2*r2*r2)
+ T(2)*distortion[2]*xp*yp + distortion[3]*(r2 + T(2)*xp*xp);
T ypd = yp * (T(1) + distortion[0]*r2 + distortion[1]*r2*r2 + distortion[4]*r2*r2*r2)
+ T(2)*distortion[3]*xp*yp + distortion[2]*(r2 + T(2)*yp*yp);
x = intrinsic[0]*xpd + intrinsic[2];
y = intrinsic[1]*ypd + intrinsic[3];
}
struct MonoCumulativeProjectionConstraint {
public:
MonoCumulativeProjectionConstraint(const std::vector<std::vector<cv::Vec3f> >& m,
const std::vector<std::vector<cv::Vec2f> >& i
)
: m_ModelPoints(m)
, m_ImagePoints(i)
{
}
/**
* Lets define
* intrinsic (4) (0-3)
* distortion (5) (4-8)
* initial transform (9-14)
* Extrinsic parameters for each camera (15 onwards)
*/
template <typename T>
bool operator()(T const* const* parameters,
T* residuals) const {
unsigned long long int residualCounter = 0;
for (std::vector<std::vector<cv::Vec3f> >::size_type i = 0; i < m_ModelPoints.size(); i++)
{
for (std::vector<cv::Point3f>::size_type p = 0; p < m_ModelPoints[i].size(); p++)
{
T m[3];
m[0] = T(static_cast<double>(m_ModelPoints[i][p](0)));
m[1] = T(static_cast<double>(m_ModelPoints[i][p](1)));
m[2] = T(static_cast<double>(m_ModelPoints[i][p](2)));
// Initial transform
T l[3];
ceres::AngleAxisRotatePoint(parameters[2], m, l);
l[0] += (parameters[2])[3];
l[1] += (parameters[2])[4];
l[2] += (parameters[2])[5];
// Now accumulate all the intra-camera offsets.
for (int j = 0; j <= i; j++)
{
T tmp[3];
tmp[0] = l[0];
tmp[1] = l[1];
tmp[2] = l[2];
ceres::AngleAxisRotatePoint(&((parameters[3])[j*6 + 0]), tmp, l);
l[0] += (parameters[3])[j*6 + 3];
l[1] += (parameters[3])[j*6 + 4];
l[2] += (parameters[3])[j*6 + 5];
}
T lx;
T ly;
ProjectToPoint<T>(parameters[0],
parameters[1],
l,
lx,
ly
);
residuals[residualCounter++] = lx - T(static_cast<double>(m_ImagePoints[i][p](0)));
residuals[residualCounter++] = ly - T(static_cast<double>(m_ImagePoints[i][p](1)));
}
}
return true;
}
std::vector<std::vector<cv::Vec3f> > m_ModelPoints;
std::vector<std::vector<cv::Vec2f> > m_ImagePoints;
};
//-----------------------------------------------------------------------------
double CeresMonoCameraCalibration(const std::vector<std::vector<cv::Vec3f> >& modelVectors3D,
const std::vector<std::vector<cv::Vec2f> >& imageVectors2D,
cv::Mat& intrinsic,
cv::Mat& distortion,
std::vector<cv::Mat>& rvecs,
std::vector<cv::Mat>& tvecs
)
{
unsigned long long int numberOfPoints = 0;
for (std::vector<std::vector<cv::Vec3f> >::size_type i = 0; i < modelVectors3D.size(); i++)
{
numberOfPoints += modelVectors3D[i].size();
}
const unsigned int numberOfParameters = 4 // intrinsic
+ 5 // distortion (not optimised)
+ 6 // first left transformation (not optimised)
+ 6*rvecs.size(); // extrinsics for each left-hand camera
double *parameters = new double[numberOfParameters];
unsigned int parameterCounter = 0;
parameters[parameterCounter++] = intrinsic.at<double>(0, 0);
parameters[parameterCounter++] = intrinsic.at<double>(1, 1);
parameters[parameterCounter++] = intrinsic.at<double>(0, 2);
parameters[parameterCounter++] = intrinsic.at<double>(1, 2);
parameters[parameterCounter++] = distortion.at<double>(0, 0);
parameters[parameterCounter++] = distortion.at<double>(0, 1);
parameters[parameterCounter++] = distortion.at<double>(0, 2);
parameters[parameterCounter++] = distortion.at<double>(0, 3);
parameters[parameterCounter++] = distortion.at<double>(0, 4);
parameters[parameterCounter++] = rvecs[0].at<double>(0, 0);
parameters[parameterCounter++] = rvecs[0].at<double>(0, 1);
parameters[parameterCounter++] = rvecs[0].at<double>(0, 2);
parameters[parameterCounter++] = tvecs[0].at<double>(0, 0);
parameters[parameterCounter++] = tvecs[0].at<double>(0, 1);
parameters[parameterCounter++] = tvecs[0].at<double>(0, 2);
// So the parameters for the first camera that are optimised, start at the identity transform.
cv::Matx44d identity = cv::Matx44d::eye();
cv::Mat firstCameraRotationVector = cvCreateMat(1, 3, CV_64FC1);
cv::Mat firstCameraTranslationVector = cvCreateMat(1, 3, CV_64FC1);
niftk::MatrixToRodrigues(identity, firstCameraRotationVector, firstCameraTranslationVector);
parameters[parameterCounter++] = firstCameraRotationVector.at<double>(0, 0);
parameters[parameterCounter++] = firstCameraRotationVector.at<double>(0, 1);
parameters[parameterCounter++] = firstCameraRotationVector.at<double>(0, 2);
parameters[parameterCounter++] = firstCameraTranslationVector.at<double>(0, 0);
parameters[parameterCounter++] = firstCameraTranslationVector.at<double>(0, 1);
parameters[parameterCounter++] = firstCameraTranslationVector.at<double>(0, 2);
// Then for all other cameras, we go relative to the previous camera.
for (int i = 1; i < rvecs.size(); i++)
{
cv::Matx44d previousCamera = niftk::RodriguesToMatrix(rvecs[i-1], tvecs[i-1]);
cv::Matx44d currentCamera = niftk::RodriguesToMatrix(rvecs[i], tvecs[i]);
cv::Matx44d previousCameraToCurrentCamera = currentCamera * previousCamera.inv();
cv::Mat previousCameraToCurrentCameraRotationVector = cvCreateMat(1, 3, CV_64FC1);
cv::Mat previousCameraToCurrentCameraTranslationVector = cvCreateMat(1, 3, CV_64FC1);
niftk::MatrixToRodrigues(previousCameraToCurrentCamera,
previousCameraToCurrentCameraRotationVector,
previousCameraToCurrentCameraTranslationVector);
parameters[parameterCounter++] = previousCameraToCurrentCameraRotationVector.at<double>(0, 0);
parameters[parameterCounter++] = previousCameraToCurrentCameraRotationVector.at<double>(0, 1);
parameters[parameterCounter++] = previousCameraToCurrentCameraRotationVector.at<double>(0, 2);
parameters[parameterCounter++] = previousCameraToCurrentCameraTranslationVector.at<double>(0, 0);
parameters[parameterCounter++] = previousCameraToCurrentCameraTranslationVector.at<double>(0, 1);
parameters[parameterCounter++] = previousCameraToCurrentCameraTranslationVector.at<double>(0, 2);
}
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_SCHUR;
options.minimizer_progress_to_stdout = true;
ceres::Problem problem;
ceres::DynamicAutoDiffCostFunction<MonoCumulativeProjectionConstraint> *costFunction =
new ceres::DynamicAutoDiffCostFunction<MonoCumulativeProjectionConstraint>(
new MonoCumulativeProjectionConstraint(modelVectors3D, imageVectors2D)
);
std::vector<double*> parameterBlocks;
parameterBlocks.push_back(&parameters[0]);
costFunction->AddParameterBlock(4);
parameterBlocks.push_back(&parameters[4]);
costFunction->AddParameterBlock(5);
parameterBlocks.push_back(&parameters[9]);
costFunction->AddParameterBlock(6);
parameterBlocks.push_back(&parameters[15]);
costFunction->AddParameterBlock(6 * modelVectors3D.size());