void GSystem::calcDerivative_MomentumCOM_Dq_Ddq(RMatrix &DHcDq_, RMatrix &DHcDdq_)
{
int i;
std::list<GBody *>::iterator iter_pbody;
std::list<GCoordinate *>::iterator iter_pcoord;
std::vector<SE3> M(pBodies.size());
std::vector<GConstraintJointLoop::JOINTLOOP_CONSTRAINT_TYPE> jlc_type(pBodies.size());
for (i=0, iter_pbody = pBodies.begin(); iter_pbody != pBodies.end(); i++, iter_pbody++) {
// save previous settings
M[i] = (*iter_pbody)->fwdJointChain.M1;
jlc_type[i] = (*iter_pbody)->fwdJointChain.jointLoopConstraintType;
// prerequisites for (*iter_pbody)->getDerivative_MomentumGlobal_Dq(...) and (*iter_pbody)->getDerivative_MomentumGlobal_Ddq(...)
(*iter_pbody)->fwdJointChain.setM(SE3());
(*iter_pbody)->fwdJointChain.jointLoopConstraintType = GConstraintJointLoop::JOINTLOOP_ORIENTATION_POSITION;
(*iter_pbody)->fwdJointChain.update_J();
}
// calculate the derivatives
Vec3 p = getPositionCOMGlobal();
dse3 Hg = getMomentumGlobal();
RMatrix DHgDq(6, getNumCoordinates()), DHgDdq(6, getNumCoordinates());
dse3 DHgDqi, DHgDdqi;
for (i=0, iter_pcoord = pCoordinates.begin(); iter_pcoord != pCoordinates.end(); i++, iter_pcoord++) {
DHgDqi = getDerivative_MomentumGlobal_Dq(*iter_pcoord);
DHgDdqi = getDerivative_MomentumGlobal_Ddq(*iter_pcoord);
//DHgDq.Push(0, i, convert_to_RMatrix(DHgDqi));
//DHgDdq.Push(0, i, convert_to_RMatrix(DHgDdqi));
matSet(&DHgDq[6*i], DHgDqi.GetArray(), 6);
matSet(&DHgDdq[6*i], DHgDdqi.GetArray(), 6);
}
RMatrix DdAdDq_Hg(6, getNumCoordinates());
Vec3 DpDqi;
for (i=0, iter_pcoord = pCoordinates.begin(); iter_pcoord != pCoordinates.end(); i++, iter_pcoord++) {
DpDqi = getDerivative_PositionCOMGlobal_Dq(*iter_pcoord);
//DdAdDq_Hg.Push(0, i, convert_to_RMatrix(dAd(SE3(p), dad(se3(Vec3(0,0,0),DpDqi), Hg))));
matSet(&DdAdDq_Hg[6*i], dAd(SE3(p), dad(se3(Vec3(0,0,0),DpDqi), Hg)).GetArray(), 6);
}
DHcDq_ = dAd(SE3(p), DHgDq) + DdAdDq_Hg;
DHcDdq_ = dAd(SE3(p), DHgDdq);
// restore the previous settings
for (i=0, iter_pbody = pBodies.begin(); iter_pbody != pBodies.end(); i++, iter_pbody++) {
(*iter_pbody)->fwdJointChain.setM(M[i]);
(*iter_pbody)->fwdJointChain.setJointLoopConstraintType(jlc_type[i]);
}
}
void GSystem::calcDerivative_PositionCOMGlobal_Dq(RMatrix &DpDq_)
{
int i;
std::list<GBody *>::iterator iter_pbody;
std::list<GCoordinate *>::iterator iter_pcoord;
std::vector<SE3> M(pBodies.size());
std::vector<GConstraintJointLoop::JOINTLOOP_CONSTRAINT_TYPE> jlc_type(pBodies.size());
Vec3 DpDqi;
for (i=0, iter_pbody = pBodies.begin(); iter_pbody != pBodies.end(); i++, iter_pbody++) {
// save previous settings
M[i] = (*iter_pbody)->fwdJointChain.M1;
jlc_type[i] = (*iter_pbody)->fwdJointChain.jointLoopConstraintType;
// prerequisites for (*iter_pbody)->getDerivative_PositionCOMGlobal_Dq(...)
(*iter_pbody)->fwdJointChain.setM(SE3());
(*iter_pbody)->fwdJointChain.jointLoopConstraintType = GConstraintJointLoop::JOINTLOOP_ORIENTATION_POSITION;
(*iter_pbody)->fwdJointChain.update_J();
}
// calculate the derivative
DpDq_.SetZero(3, getNumCoordinates());
for (i=0, iter_pcoord = pCoordinates.begin(); iter_pcoord != pCoordinates.end(); i++, iter_pcoord++) {
DpDqi = getDerivative_PositionCOMGlobal_Dq(*iter_pcoord);
//DpDq_.Push(0, i, convert_to_RMatrix(DpDqi));
matSet(&DpDq_[3*i], DpDqi.GetArray(), 3);
}
// restore the previous settings
for (i=0, iter_pbody = pBodies.begin(); iter_pbody != pBodies.end(); i++, iter_pbody++) {
(*iter_pbody)->fwdJointChain.setM(M[i]);
(*iter_pbody)->fwdJointChain.setJointLoopConstraintType(jlc_type[i]);
}
}
void GSystem::calcDerivative_MomentumGlobal_Dq_Ddq(std::vector<GCoordinate*> pCoordinates_, RMatrix &DHgDq_, RMatrix &DHgDdq_)
{
int i;
std::list<GBody *>::iterator iter_pbody;
std::vector<GCoordinate *>::iterator iter_pcoord;
std::vector<SE3> M(pBodies.size());
std::vector<GConstraintJointLoop::JOINTLOOP_CONSTRAINT_TYPE> jlc_type(pBodies.size());
dse3 DHDqi, DHDdqi;
for (i=0, iter_pbody = pBodies.begin(); iter_pbody != pBodies.end(); i++, iter_pbody++) {
// save previous settings
M[i] = (*iter_pbody)->fwdJointChain.M1;
jlc_type[i] = (*iter_pbody)->fwdJointChain.jointLoopConstraintType;
// prerequisites for (*iter_pbody)->getDerivative_MomentumGlobal_Dq(...) and (*iter_pbody)->getDerivative_MomentumGlobal_Ddq(...)
(*iter_pbody)->fwdJointChain.setM(SE3());
(*iter_pbody)->fwdJointChain.jointLoopConstraintType = GConstraintJointLoop::JOINTLOOP_ORIENTATION_POSITION;
(*iter_pbody)->fwdJointChain.update_J();
}
// calculate the derivatives
DHgDq_.SetZero(6, int(pCoordinates_.size()));
DHgDdq_.SetZero(6, int(pCoordinates_.size()));
for (i=0, iter_pcoord = pCoordinates_.begin(); iter_pcoord != pCoordinates_.end(); i++, iter_pcoord++) {
DHDqi = getDerivative_MomentumGlobal_Dq(*iter_pcoord);
DHDdqi = getDerivative_MomentumGlobal_Ddq(*iter_pcoord);
//DHgDq_.Push(0, i, convert_to_RMatrix(DHDqi));
//DHgDdq_.Push(0, i, convert_to_RMatrix(DHDdqi));
matSet(&DHgDq_[6*i], DHDqi.GetArray(), 6);
matSet(&DHgDdq_[6*i], DHDdqi.GetArray(), 6);
}
// restore the previous settings
for (i=0, iter_pbody = pBodies.begin(); iter_pbody != pBodies.end(); i++, iter_pbody++) {
(*iter_pbody)->fwdJointChain.setM(M[i]);
(*iter_pbody)->fwdJointChain.setJointLoopConstraintType(jlc_type[i]);
}
}
camcalib::camcalib()
{
rectification = new Rectify*[2];
rectification[0] = new Rectify();
rectification[1] = new Rectify();
rectification_loaded = false;
v_shift = 0;
double intCalib[] = {
340, 0, 330,
0, 340, 94,
0, 0, 1
};
intrinsicCalibration_right = cvCreateMat(3,3,CV_64F);
matSet(intrinsicCalibration_right,intCalib);
intrinsicCalibration_left = cvCreateMat(3,3,CV_64F);
matSet(intrinsicCalibration_left,intCalib);
double distortion[] = {
-0.38407, 0.13186, 0.00349, -0.00392
};
distortion_left = cvCreateMat(1,4,CV_64F);
matSet(distortion_left, distortion);
distortion_right = cvCreateMat(1,4,CV_64F);
matSet(distortion_right, distortion);
double trans[] = { -0.575, 0, 0 };
extrinsicTranslation = cvCreateMat(3,1,CV_64F);
matSet(extrinsicTranslation, trans);
disparityToDepth = cvCreateMat(4,4,CV_64F);
extrinsicRotation = cvCreateMat(3,1,CV_64F);
fundamentalMatrix = cvCreateMat(3,3,CV_64F);
essentialMatrix = cvCreateMat(3,3,CV_64F);
double initPose[] = {
1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 0
};
pose = cvCreateMat(4,4,CV_64F);
matSet(pose, initPose);
}
QVi RegionService::countSum(Mat srcBGRImg, QLP region) {
MatSet matSet(srcBGRImg);
return countSum(matSet, region);
}
QVi RegionService::calcAverage(Mat srcBGRImg, QLP region) {
MatSet matSet(srcBGRImg);
return calcAverage(matSet, region);
}
/**
* 指定領域の9チャンネルのHistogramを作成して返す
*
*/
QVis RegionService::createHistogram(Mat srcBGRImg, QLP region) {
MatSet matSet(srcBGRImg);
return createHistogram(matSet, region);
}
bool GSystemIK::buildConstrIK_dq(RMatrix &J, RMatrix &V, std::vector<GBody*> pbodies, std::vector<Vec3> pos, std::vector<se3> V_target, std::vector< std::vector<int> > idxC)
{
int i, j, k;
int cnt; // a counter
int nb; // number of bodies
int ncik; // number of IK constraints
std::list<GCoordinate*>::iterator iter_pcoord, iter_pcoord2;
nb = int(pbodies.size());
if ( pos.size() != (size_t)nb || V_target.size() != (size_t)nb || idxC.size() != (size_t)nb ) return false;
// counts the number of IK constraints
ncik = 0;
for (i=0; i<nb; i++) {
for ( j=0; j<int(idxC[i].size()); j++) {
if ( idxC[i][j] < 0 || idxC[i][j] > 5 ) return false;
}
ncik += int(idxC[i].size());
}
J.SetZero(ncik, getNumCoordinates());
V.SetZero(ncik, 1);
// build J, V
cnt = 0;
for (i=0; i<nb; i++) {
// update Jacobian
pbodies[i]->fwdJointChain.setM(SE3(pos[i]));
pbodies[i]->fwdJointChain.setJointLoopConstraintType(GConstraintJointLoop::JOINTLOOP_ORIENTATION_POSITION);
pbodies[i]->fwdJointChain.update_J();
// transformed Jacobian, Jg = [R 0; 0 R] * J
RMatrix Jg(pbodies[i]->fwdJointChain.J.RowSize(), pbodies[i]->fwdJointChain.J.ColSize());
//RMatrix R = convert_to_RMatrix(pbodies[i]->T_global.GetRotation());
RMatrix R(3,3); matSet(R.GetPtr(), pbodies[i]->T_global.GetRotation().GetArray(), 9);
Jg.Push(0, 0, R * pbodies[i]->fwdJointChain.J.Sub(0, 2, 0, pbodies[i]->fwdJointChain.J.ColSize()-1));
Jg.Push(3, 0, R * pbodies[i]->fwdJointChain.J.Sub(3, 5, 0, pbodies[i]->fwdJointChain.J.ColSize()-1));
// build J
for (j=0, iter_pcoord = pbodies[i]->fwdJointChain.pCoordinates.begin(); iter_pcoord != pbodies[i]->fwdJointChain.pCoordinates.end(); j++, iter_pcoord++) {
// find index of pbodies[i]->fwdJointChain.pCoordinates[j] in pCoordinates
int idx = -1;
for (k=0, iter_pcoord2 = pCoordinates.begin(); iter_pcoord2 != pCoordinates.end(); k++, iter_pcoord2++) {
if ( *iter_pcoord2 == *iter_pcoord ) { idx = k; break; }
}
if ( idx < 0 ) return false;
// insert j-th column of the body Jacobian to the right place
for (k=0; k<int(idxC[i].size()); k++) {
J(cnt+k, idx) = Jg(idxC[i][k], j);
}
}
// build V
for (j=0; j<int(idxC[i].size()); j++) {
V(cnt+j, 0) = V_target[i][idxC[i][j]];
}
cnt += int(idxC[i].size());
}
return true;
}
